function Derive_EoM()
% Derive_EoM()
%
% This function generates the equations of motion for what I will call the
% "Retractable Double Pendulum" model of walking. It uses the Matlab
% symbolic toolbox to generate the equations of motion, and then
% automatically writes them to a file.
%
% The model consists of a point mass at each foot and at the hip. The feet
% are connected to this hip by an extensible, actuated, leg. Each foot has
% an 'ankle actuator' to provide a control torque, and there is another
% torque actuator at the hip, connecting the two legs.
%
% Written by Matthew Kelly
% November 29, 2013
% Cornell University
%
% See also WRITE_CONTINUOUSDYNAMICS
clc; clear; commandwindow;
addpath ../Shared
Directory = '../computerGeneratedCode';  %Write all code in this directory
disp('Running Derive_EoM...')
disp(' -> Defining Model');
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Model                                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% This model of walking is comprised of three point masses: one at each
% foot and one at the hip. The equations are derived for three phases of
% motion: Flight, Single Stance, and Double Stance. The lets in this
% walking model are massless and have variable length. There are five
% controls available to the system: force actuator in each leg, foot torque
% when foot is in contact with the ground, and hip torque. There are six
% degrees of freedom in this model: position of foot one (x1,y1), position
% of the hip (x0,y0), and position of foot two (x2, y2).

% The position of the hip
x0 = sym('x0','real'); % Horizontal position of the hip
y0 = sym('y0','real'); % Vertical position of the hip

% The position of foot one
x1 = sym('x1','real'); % Horizontal position of foot one
y1 = sym('y1','real'); % Vertical position of foot one

% The position of foot two
x2 = sym('x2','real'); % Horizontal position of foot two
y2 = sym('y2','real'); % Vertical position of foot two

% Each leg has a small mass at the foot and a large mass at the hip. Since
% the two hip masses are coincident, they are treated as a single mass.
m1 = sym('m1','real');  % Mass of foot one
m2 = sym('m2','real');  % Mass of foot two
M = sym('M','real');  % Mass of the hip of the robot

% The system experiences a constant gravitational acceleration:
g = sym('g','real');

% Axial Force along each leg. This force is considered to be an actuator
% input to the system. Compression is positive.
F1 = sym('F1','real'); % Force in the stance leg
F2 = sym('F2','real'); % Force in the swing leg

% Constraint force normal to each leg. Connects ankle torques to system
N1 = sym('N1','real'); % Constraint force at foot one
N2 = sym('N2','real'); % constraint force between legs

% There is a torque motor connecting the two legs, and an ankle motor on
% the stance leg.
T1 = sym('T1','real'); % Ankle Torque, acting on leg one
T2 = sym('T2','real'); % Ankle Torque, acting on leg two
Thip = sym('Thip','real'); % Hip torque. From leg one acting on leg two

% Ground contact forces at each foot
H1 = sym('H1','real'); % Horizontal contact force at foot one
V1 = sym('V1','real'); % Vertical contact force at foot one
H2 = sym('H2','real'); % Horizontal contact force at foot two
V2 = sym('V2','real'); % Vertical contact force at foot wto

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        State Derivatives                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% The first time derivative of each state. These are considered known.
% The position of the hip
dx0 = sym('dx0','real'); % Horizontal velocity of the hip
dy0 = sym('dy0','real'); % Vertical velocity of the hip
dx1 = sym('dx1','real'); % Horizontal velocity of foot one
dy1 = sym('dy1','real'); % Vertical velocity of foot one
dx2 = sym('dx2','real'); % Horizontal velocity of foot two
dy2 = sym('dy2','real'); % Vertical velocity of foot two

% The second time derivative of each state. Goal is to find these.
ddx0 = sym('ddx0','real'); % Horizontal acceleration of the hip
ddy0 = sym('ddy0','real'); % Vertical acceleration of the hip
ddx1 = sym('ddx1','real'); % Horizontal acceleration of foot one
ddy1 = sym('ddy1','real'); % Vertical acceleration of foot one
ddx2 = sym('ddx2','real'); % Horizontal acceleration of foot two
ddy2 = sym('ddy2','real'); % Vertical acceleration of foot two

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                 Coordinate System & Kinematics                          %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% Inertial reference frame:
i = [1;0;0];  %Positive horizontal axis
j = [0;1;0];  %Positive vertical axis
k = [0;0;1];  %Positive lateral axis

%Relative change in each ordinate along the legs
X1 = (x1-x0); dX1 = (dx1-dx0);
Y1 = (y1-y0); dY1 = (dy1-dy0);
X2 = (x2-x0); dX2 = (dx2-dx0);
Y2 = (y2-y0); dY2 = (dy2-dy0);

%length of both legs:
% This is a commonly used expression that is costly to calculate, so I
% numerically calculate it as an intermediate step.
L1 = sym('L1','real');
L2 = sym('L2','real');
dL1 = sym('dL1','real');
dL2 = sym('dL2','real');

%Angles of both legs, as measured in the k direction from the -j axis:
% th1 = sym('th1','real');
% th2 = sym('th2','real');
dth1 = sym('dth1','real');
dth2 = sym('dth2','real');

% Matlab symbolic toolbox was not being friendly, so I did a small amount
% of the math out by hand. I believe the following to be true:
%
%  L^2 = x^2 + y^2
% (d/dt)(L) = (x*dx + y*dy)/L
% (d/dt)(atan2(x,y)) = (dx*y - x*dy)/L^2
%

Kinematics.L1 = sqrt(X1^2 + Y1^2);
Kinematics.L2 = sqrt(X2^2 + Y2^2);

Kinematics.dL1 = (X1*dX1 + Y1*dY1)/L1;
Kinematics.dL2 = (X2*dX2 + Y2*dY2)/L2;

Kinematics.th1 = atan2(X1,Y1);
Kinematics.th2 = atan2(X2,Y2);
Kinematics.dth1 = (dX1*Y1 - X1*dY1)/L1^2;
Kinematics.dth2 = (dX2*Y2 - X2*dY2)/L2^2;

% Unit vectors pointing from the hip to foot one (a1) and it's normal (b1)
a1 = (X1*i + Y1*j)/L1;
b1 = (-Y1*i + X1*j)/L1;

% Unit vectors pointing from the hip to foot two (a2) and it's normal (b2)
a2 = (X2*i + Y2*j)/L2;
b2 = (-Y2*i + X2*j)/L2;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Position Vectors                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% Point 0 = Hip
% Point 1 = Foot One
% Point 2 = Foot Two

r0 = x0*i + y0*j;         % Position of Hip(Absolute)
r1 = x1*i + y1*j;         % Position of Foot One (Absolute)
r2 = x2*i + y2*j;         % Position of Foot Two (Absolute)

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Position Derivatives                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

dr0 = dx0*i + dy0*j;         % Velocity of Hip(Absolute)
dr1 = dx1*i + dy1*j;         % Velocity of Foot One (Absolute)
dr2 = dx2*i + dy2*j;         % Velocity of Foot Two (Absolute)

ddr0 = ddx0*i + ddy0*j;         % Acceleration of Hip(Absolute)
ddr1 = ddx1*i + ddy1*j;         % Acceleration of Foot One (Absolute)
ddr2 = ddx2*i + ddy2*j;         % Acceleration of Foot Two (Absolute)

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Linear Momentum Balance on Foot One                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

sum_of_forces = simplify(V1*j + H1*i + N1*b1 - m1*g*j + F1*a1);
linear_momentum_rate = m1*ddr1;

LMB_F1 = simplify(sum_of_forces - linear_momentum_rate);
LMB_F1_i = dot(LMB_F1,i);
LMB_F1_j = dot(LMB_F1,j);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Linear Momentum Balance on Foot Two                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

sum_of_forces = simplify(V2*j + H2*i + N2*b2 - m2*g*j + F2*a2);
linear_momentum_rate = m2*ddr2;

LMB_F2 = simplify(sum_of_forces - linear_momentum_rate);
LMB_F2_i = dot(LMB_F2,i);
LMB_F2_j = dot(LMB_F2,j);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Linear Momentum Balance on Hip                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

sum_of_forces = simplify(-F1*a1 - N1*b1 - F2*a2 - N2*b2 - M*g*j);
linear_momentum_rate = M*ddr0;

LMB_H = simplify(sum_of_forces - linear_momentum_rate);
LMB_H_i = dot(LMB_H,i);
LMB_H_j = dot(LMB_H,j);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%               Angular Momentum Balance on Leg One                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

sum_of_torques = (T1 + Thip - N1*L1)*k;
angular_momentum_rate = 0*k;   %Leg One has no mass

AMB_L1_k = dot(simplify(sum_of_torques - angular_momentum_rate),k);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%               Angular Momentum Balance on Leg One                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

sum_of_torques = (T2 - Thip - N2*L2)*k;
angular_momentum_rate = 0*k;   %Leg Two has no mass

AMB_L2_k = dot(simplify(sum_of_torques - angular_momentum_rate),k);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%      Collect Implicit Equations of Motion and Phase constraints         %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% All phases of motion share these equations. An additional four equations
% are required to solve the system, which come from the 'definition' of
% each phase of motion.

Physics = [         LMB_F1_i;
                    LMB_F1_j;
                    LMB_F2_i;
                    LMB_F2_j;
                    LMB_H_i;
                    LMB_H_j;
                    AMB_L1_k;
                    AMB_L2_k    ];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Solve Flight Dynamics                            %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% During flight, the contact forces are all equal to zero. Treat as knowns.
% %             H1==0;
% %             V1==0;
% %             H2==0;
% %             V2==0;

Unknowns = [    ddx0;    ddy0;
                ddx1;    ddy1;
                ddx2;    ddy2;
                N1;     N2];

disp(' -> Solving Flight Dynamics')
Soln = jacobSolve(Physics,Unknowns);

Dyn.Flight = Soln;
Dyn.Flight.H1 = sym('0');
Dyn.Flight.V1 = sym('0');
Dyn.Flight.H2 = sym('0');
Dyn.Flight.V2 = sym('0');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                 Solve Single Stance One Dynamics                        %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% During single stance one, contact forces at Foot Two are equal to zero.
% Additionally, we know that the velocity and acceleration of Foot One are
% both also equal to zero.
% %             ddx1==0;
% %             ddy1==0;
% %             H2==0;
% %             V2==0;

Unknowns = [    ddx0;   ddy0;
                H1;     V1
                ddx2;   ddy2;
                N1;     N2];

disp(' -> Solving Single Stance One Dynamics')
Soln = jacobSolve(Physics,Unknowns);

Dyn.SingleOne = Soln;
Dyn.SingleOne.ddx1 = sym('0');
Dyn.SingleOne.ddy1 = sym('0');
Dyn.SingleOne.H2 = sym('0');
Dyn.SingleOne.V2 = sym('0');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                 Solve Single Stance Two Dynamics                        %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% During single stance two, contact forces at Foot One are equal to zero.
% Additionally, we know that the velocity and acceleration of Foot Two are
% both also equal to zero.
%
%             ddx2 == 0;
%             ddy2 == 0;
%             H1==0;
%             V1==0;

Unknowns = [    ddx0;   ddy0;
                ddx1;   ddy1;
                H2;     V2;
                N1;     N2];

disp(' -> Solving Single Stance Two Dynamics')
Soln = jacobSolve(Physics,Unknowns);

Dyn.SingleTwo = Soln;
Dyn.SingleTwo.ddx2 = sym('0');
Dyn.SingleTwo.ddy2 = sym('0');
Dyn.SingleTwo.H1 = sym('0');
Dyn.SingleTwo.V1 = sym('0');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                   Solve Double Stance Dynamics                          %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% During double stance, both feet remain stationary.
% Additionally, we know that the velocity and acceleration of Foot One are
% both also equal to zero.
%               ddx1 == 0;
%               ddy1 == 0;
%               ddx2 == 0;
%               ddy2 == 0;

% In Double Stance, Both feet remain stationary. Add constraint equation.
% Note that the constraint is on acceleration, NOT velocity. This means
% that these equations will only yield the desired solutions if the initial
% velocity of Foot Two == 0;

Unknowns = [    ddx0;   ddy0
                N1;     N2;
                H1;     V1;
                H2;     V2];

disp(' -> Solving Double Stance Dynamics')
Soln = jacobSolve(Physics,Unknowns);

Dyn.Double = Soln;
Dyn.Double.ddx1 = sym('0');
Dyn.Double.ddy1 = sym('0');
Dyn.Double.ddx2 = sym('0');
Dyn.Double.ddy2 = sym('0');

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%              Write Continuous Dynamics Function Files                   %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Continuous Dynamics Functions')

States = cell(12,2);
States(1,:) = {'x0','(m) Hip horizontal position'};
States(2,:) = {'y0','(m) Hip vertical position'};
States(3,:) = {'x1','(m) Foot One horizontal position'};
States(4,:) = {'y1','(m) Foot One vertical position'};
States(5,:) = {'x2','(m) Foot Two horizontal position'};
States(6,:) = {'y2','(m) Foot Two vertical position'};

States(7,:) = {'dx0','(m/s) Hip horizontal velocity'};
States(8,:) = {'dy0','(m/s) Hip vertical velocity'};
States(9,:) = {'dx1','(m/s) Foot One horizontal velocity'};
States(10,:) = {'dy1','(m/s) Foot One vertical velocity'};
States(11,:) = {'dx2','(m/s) Foot Two horizontal velocity'};
States(12,:) = {'dy2','(m/s) Foot Two vertical velocity'};

Contacts = cell(4,2);
Contacts(1,:) = {'H1','(N) Foot One, horizontal contact force'};
Contacts(2,:) = {'V1','(N) Foot One, vertical contact force'};
Contacts(3,:) = {'H2','(N) Foot Two, horizontal contact force'};
Contacts(4,:) = {'V2','(N) Foot Two, vertical contact force'};

Actuators = cell(5,2);
Actuators(1,:) = {'F1','(N) Compresive axial force in Leg One'};
Actuators(2,:) = {'F2','(N) Compresive axial force in Leg Two'};
Actuators(3,:) = {'T1','(Nm) External torque applied to Leg One'};
Actuators(4,:) = {'T2','(Nm) External torque applied to Leg Two'};
Actuators(5,:) = {'Thip','(Nm) Torque acting on Leg Two from Leg One'};

Parameters = cell(4,2);
Parameters(1,:) = {'m1','(kg) Foot One mass'};
Parameters(2,:) = {'m2','(kg) Foot Two mass'};
Parameters(3,:) = {'M','(kg) Hip mass'};
Parameters(4,:) = {'g','(m/s^2) Gravity'};

CommonExpressions = cell(2,2);
CommonExpressions(1,:) = {'L1s','L1.^2'};
CommonExpressions(2,:) = {'L2s','L2.^2'};

FileData = cell(3,4);
FileData(1,:) = {'Flight','dynamics_flight',...
    'Dymanics Model: retractable double pendulum biped',...
    'Motion Phase: Flight'};
FileData(2,:) = {'SingleOne','dynamics_singleStanceOne',...
    'Dymanics Model: retractable double pendulum biped',...
    'Motion Phase: Single Stance One'};
FileData(3,:) = {'SingleTwo','dynamics_singleStanceTwo',...
    'Dymanics Model: retractable double pendulum biped',...
    'Motion Phase: Single Stance Two'};
FileData(4,:) = {'Double','dynamics_doubleStance',...
    'Dymanics Model: retractable double pendulum biped',...
    'Motion Phase: Double Stance'};

FileWritingSetup = Make_Struct(Dyn,States,Contacts,Actuators,...
    Kinematics,CommonExpressions,Parameters,FileData,Directory);

Write_ContinuousDynamics(FileWritingSetup);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Find System Energy                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

Energy.Potential.m1 = simplify(m1*g*dot(r1,j));
Energy.Potential.m2 = simplify(m2*g*dot(r2,j));
Energy.Potential.M = simplify(M*g*dot(r0,j));

Energy.Kinetic.m1 = simplify(0.5*m1*norm(dr1).^2);
Energy.Kinetic.m2 = simplify(0.5*m2*norm(dr2).^2);
Energy.Kinetic.M = simplify(0.5*M*norm(dr0)^2);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Write Energy Function                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Kinematics Function')

FileWritingSetup = Make_Struct(Energy,States,Parameters,Directory);

Write_Energy(FileWritingSetup);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Write Power Function                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Actuator Power Function')

Power.legOne = F1*dL1;
Power.legTwo = F2*dL2;
Power.ankleOne = T1*dth1;
Power.ankleTwo = T2*dth2;
Power.hip = Thip*(dth2-dth1);

FileWritingSetup = Make_Struct(Power,Kinematics,...
    States,Actuators,Directory);

Write_ActuatorPower(FileWritingSetup);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Write Kinematics Function                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Kinematics Function')

FileWritingSetup = Make_Struct(Kinematics, States, Directory);

Write_Kinematics(FileWritingSetup);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Impact Equations                            %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% Assume that any transitions between phases can be modeled by external
% impluses being applied to either Foot One or Foot Two.
%
% Each of the point masses in the model is connected to the others through
% force and torque actuators. Assume that at any instant in time there is a
% finite force or torque being provided by each of those actuators. In the
% limit as the duration of the impact goes to zero, the impulse transfered
% across those actuators also goes to zero.
%
% Thus, I conclude that the collision maps can be applied by solving the
% kinematic constraints before and after the collision.
%
% Since the masses in this system are essientally decoupled from eachother
% as far as impacts are concerned, the impact map can be simplified to the
% following statement: "The state is unchanged, except for the velocity of
% the mass that strikes the ground, which should go to zero".

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Write Impact Map Function Files                      %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Phase Map Function')

FileWritingSetup = ...
    Make_Struct(States, Directory);

Write_PhaseMap(FileWritingSetup);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Write Conversion Function                            %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
disp(' -> Writing Conversion Function')

FileWritingSetup = Make_Struct(States,Actuators,Contacts,Directory);

Write_Convert(FileWritingSetup);

disp('DONE!');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%                    SUB-FUNCTIONS                                  %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Soln = jacobSolve(Equations,Unknowns)
%
% FUNCTION:
%   Solve a nonlinear system of equations by assuming that it is linear in
%   the unknown variables (which is true for these mechanics problems)
%
% ARGUMENTS:
%   Equations = [Nx1] vector of symbolic expressions that are equal to zero
%   Unknowns = [Nx1] vector of symbolic variables to solve Equations for
%
% RETURNS:
%   Soln = a struct with a field for each unknown
%
% The matlab solve command seems to have a problem with solving large
% systems of non-linear equations. In the case of classical mechanics
% problems, it turns out that these systems are not too hard to solve
% because they are actually linear in the accelerations and constraint
% forces. Assuming that this is true, then you can transform the equations
% into a linear system by taking partial derivatives. Once this step is
% done, then matlab does a great job of solving the linear system.
%
% MATH:
%   Equations = 0;                  % By Definition
%   Equations = A*x + b;            % Assume: form, A independant* of x
%   A = jacobian(Equations wrt x);  %
%   b = Equations - A*x;            %
%   0 = A*x + b;                    %
%   x = -A\b;                       % Solved!
%

A = simplify(jacobian(Equations,Unknowns));
b = simplify(Equations - A*Unknowns);
x = simplify(-A\b);

for i=1:length(Unknowns)
   Soln.(char(Unknowns(i))) = x(i);
end

end


%KERNELM Trainable kernel mapping, dissimilarity representation
%
%   [W,J] = KERNELM(A,KERNEL,SELECT,P1,P2 , ...)
%    W    = A*KERNELM([],KERNEL,SELECT,P1,P2 , ...)
%    W    = A*KERNELM(KERNEL,SELECT,P1,P2 , ...)
%    K    = B*W
%
% INPUT
%   A,B         Datasets
%   KERNEL      Untrained kernel / dissimilarity representation,
%               a mapping computing proximities between objects.
%               default: Euclidean dissimilarities: PROXM('d',1)
%   SELECT      Name of object selection procedure, see below
%   P1,P2, ...  Additional parameters for SELECT
%
% OUTPUT
%   W          Mapping
%   J          Vector with indices of selected objects for representation
%   K          Kernel matrix, dissimilarity representation,
%              size [SIZE(B,1) LENGTH(J)]
%
% DESCRIPTION
% Computes the kernel mapping W for the representation objects in A. The
% computation of the kernel matrix, which is a proximity matrix (similarities
% or dissimilarities) should be defined in KERNEL by an untrained mapping
% like PROXM for predefined proximities or USERKERNEL for user specified
% proximities.
% A*KERNEL should 'train' the kernel, i.e. specify A as representation set.
% B*(A*KERNEL) should compute the kernel matrix: a dataset.
%
% The only advantage of this routine over kernel mappings defined by PROXM
% or USERKERNEL is that it includes some options for object selection
% (prototype selection) of the initial representation set.
%
% Initially, the kernel mapping has a size [SIZE(A,2) SIZE(A,1)]. For
% increased efficiency or accuracy the representation set may be reduced
% by a routine given by the string SELECT to select to objects J, using
% possibly additional parameters P1, P2, etcetera.
%
% The following choices for SELECT are supported:
%
% 'random'    random selection of P1 objects, maximum P2
% 'gendat'    [X,Y,J] = GENDAT(A,P1)
% 'kcentres'  [LAB,J] = KCENTRES(DISTM(A),P1,P2)
% 'modeseek'  [LAB,J] = MODESEEK(DISTM(A),P1)
% 'edicon'    J = EDICON(DISTM(A),P1,P2,P3)
% 'featsel'   J = +FEATSELM(A*KERNELM(A,TYPE,P),P1,P2,P3)
%
% REFERENCES
% 1. E.Pekalska, R.P.W.Duin, P.Paclik, Prototype selection for dissimilarity-
% based classification, Pattern Recognition, vol. 39, no. 2, 2006, 189-208.
% 2. E.Pekalska and R.P.W.Duin, The Dissimilarity Representation for Pattern
% Recognition, Foundations and Applications, World Scientific, 2005, 1-607.
%
% EXAMPLE
% a = gendatb;
% w = (scalem*kernelm([],'random',5)*fisherc);
% scatterd(a)
% plotc(a*w)
% plotc(a*w,'r')
% plotc(a*w,'b')
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, PROXM, USERKERNEL, KERNELC

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: kernelm.m,v 1.7 2007/07/10 08:25:29 duin Exp $

function w = kernelm(varargin)

	argin = shiftargin(varargin,'prmapping');
	argin = shiftargin(varargin,'char',2);
  argin = setdefaults(argin,[],proxm([],'d',1),[],[],[],[]);
  if mapping_task(argin,'definition')
    w = define_mapping(argin,'untrained');
    w = setname(w,'Kernel mapping');
  else % training
    varargin = cell(1,numel(argin)-3);
    [a,kernel,select,varargin{:}] = deal(argin{:});
    if isstr(kernel) % old format of call: kernelm(a,type,p,n), training
      type = kernel;
      p = select;
      if ~isempty(varargin)
        if length(varargin) > 1
          error('Wrong parameters supplied')
        end
        n = varargin{1};
      else
        n = [];
      end
      [m,k] = size(a);
      kernel = proxm([],type,p);
      w = prmapping(mfilename,'trained',{a*kernel},getlab(a),k,m);
      if ~isempty(n)
        w = w*pcam(a*w,n);
      end
      w = setname(w,'Kernel Mapping');

    elseif isa(kernel,'prmapping') && ...
           ~strcmp(getmapping_file(kernel),mfilename) % training

      a = testdatasize(a);
      a = testdatasize(a,'objects');
      isuntrained(kernel);
      [m,k] = size(a);
      %w = prmapping('kernelm','trained',{a*kernel},getlab(a),k,m);
      if isempty(select)
        w = prmapping('kernelm','trained',{a*kernel},getlab(a),k,m);
      elseif ismapping(select)
        r = a*select;
        w = prmapping('kernelm','trained',{r*kernel},getlab(a),k,size(r,1));
      else
        switch select
          case 'random'
            J = randperm(m);
            n = varargin{1};
            if isempty(n) | n > m
              error('Number of objects to be selected not given or too large')
            end
            if n < 1, n = ceil(n*m); end % fraction given
            if ~isempty(varargin{2})
              n = min(n,varargin{2});
            end
            J = J(1:n);
          case 'gendat'
            [x,y,J] = gendat(a,varargin{1});
          case 'kcentres'
            [lab,J] = kcentres(distm(a),varargin{1:2});
          case 'modeseek'
            [lab,J] = modeseek(distm(a),varargin{1});
          case 'edicon'
            J = edicon(distm(a),varargin{1:3});
          case 'featsel'
            w = prmapping('kernelm','trained',{a*kernel},getlab(a),k,m);
            J = +featselm(a*w,varargin{1:3});
          otherwise
            error('Unknown choice for object selection')
        end
        % redefine mapping with reduced representation set
        labels_out = getlab(a);
        w = prmapping('kernelm','trained',{a(J,:)*kernel},labels_out(J,:),k,length(J));
      end
      w = setname(w,'Kernel Mapping');

    else % Execution of the mapping, w will be a dataset.

      kern = getdata(kernel,1); % trained kernel is stored in datafield
      K = a*kern;
      w = setdat(a,K,kern);

    end

  end

return


function [pvec] = real_mutate(pvec, pmut_real, eta_m, ...
                                min_realvar, max_realvar)
%   This code applies polynomical mutation over pvec.

nreal = length(pvec);

if(nreal < 10)
    pvec = polymut_looped(pvec, pmut_real, eta_m, ...
                            min_realvar, max_realvar);
else
    pvec = polymut_vectorized(pvec, pmut_real, eta_m, ...
                            min_realvar, max_realvar);
end
end

function [pvec] = polymut_looped(pvec, pmut_real, eta_m, ...
                                    min_realvar, max_realvar)
% This is the original translation from the nsga-2 c-code.
nreal = length(pvec);
for j = 1:nreal
    if (rand(1) <= pmut_real)
    % if (randomperc() <= pmut_real) % SLOW !!!
        y = pvec(j);
        yl = min_realvar(j);
        yu = max_realvar(j);
        delta1 = (y-yl)/(yu-yl);
        delta2 = (yu-y)/(yu-yl);
        mut_pow = 1.0/(eta_m + 1.0);
        r = rand(1) ;
        % r = randomperc() ; % SLOW !!!
        if (r <= 0.5)
            xy = 1.0 - delta1;
            val = 2.0 * r + (1.0 - 2.0 * r) * (xy ^ (eta_m + 1.0));
            deltaq = (val ^ mut_pow) - 1.0;
        else
            xy = 1.0 - delta2;
            val = 2.0 * (1.0 - r) + 2.0 * (r - 0.5) * (xy ^ (eta_m + 1.0));
            deltaq = 1.0 - (val ^ mut_pow);
        end
        y = y + deltaq * (yu - yl);
        if (y < yl)
            y = yl;
        end
        if (y > yu)
            y = yu;
        end
        pvec(j) = y ;
    end
end
end

function [pvec] = polymut_vectorized(pvec, pmut_real, eta_m, ...
                                        min_realvar, max_realvar)
% This is the vectorized version of the above code. This code is generally
% 3 times faster than the above, more gain could be observed if the number
% of variable is bigger and the mutation rate is higher.

nreal = length(pvec);
mut_index = rand(1,nreal) < pmut_real ;
% mut_index = randompercv(1,nreal) < pmut_real ; % SLOW !!!
abs_mut_index = 1:nreal ;
abs_mut_index = abs_mut_index(mut_index);
mlen = length(abs_mut_index);

if(mlen > 0)
    eta_mv = ones(1,mlen) * eta_m ;
    yv = pvec(mut_index) ;
    ylv = min_realvar(mut_index).';
    yuv = max_realvar(mut_index).';
    delta1v = (yv - ylv) ./ (yuv - ylv);
    delta2v = (yuv - yv) ./ (yuv - ylv);
    mut_powv = 1.0 ./ (eta_mv + 1.0);
    rv = rand(1, mlen); % r2s ;
    % rv = randompercv(1, mlen); % r2s ; % SLOW !!!
    rvlthalf = rv < 0.5 ;
    valv1 = 2.0 .* rv + (1.0 - 2.0 .* rv) .* ...
                        ((1.0 - delta1v) .^ (eta_mv + 1.0));
    valv2 = 2.0 .* (1.0 - rv) + 2.0 .* (rv - 0.5) .* ...
                        ((1.0 - delta2v) .^ (eta_mv + 1.0));
    deltaqv = rvlthalf .* ((valv1 .^ mut_powv) - 1.0) + ...
             (~rvlthalf) .* (1.0 - (valv2 .^ mut_powv));
    yv = yv + deltaqv .* (yuv - ylv);
    yltyl = yv < ylv ;
    ygtyu = yv > yuv ;
    yv = (yltyl .* ylv) + (yv .* (~yltyl));
    yv = (ygtyu .* yuv) + (yv .* (~ygtyu));
    pvec = pvec' ;
    pvec(abs_mut_index.',:) = yv' ;
    pvec = pvec' ;
end
end


function [pos, tlight] = littim (tjd, idbody, pose, tlite)

% this function computes the position of a solar system body,
% as antedated for light-time.

%      tjd    = tdb julian date of observation (in)

%      idbody = id number of body, used in calls to solsys (in)

%      pose   = position vector of observer (or the geocenter),
%               with respect to origin at solar system barycenter,
%               referred to icrs axes, components in au (in)

%      tlite  = first approximation to light-time, in days (in)
%               (can be set to 0.0d0 if unknown)

%      pos    = position vector of body, with respect to origin at
%               observer (or the geocenter), referred to icrs axes,
%               components in au (out)

%      tlight = final light-time, in days (out)

% ported from NOVAS 3.1

%%%%%%%%%%%%%%%%%%%%%%%

% set light-time convergence tolerance

tol = 1.0d-9;

t0 = 0.0d0;

t1 = tjd - t0;

t2 = t1 - tlite;

% iterate to obtain correct light-time (usually converges rapidly)

for iter = 1:10

    [pos1, vel1, ierr] = solsys (t2, str2num(idbody), 0);

    [pos, tlight] = geocen (pos1, pose);

    if (ierr ~= 0)

        fprintf ('\nplace: cannot obtain coordinates of object at jd %16.8f', t0 + t2);

        return

    end

    t3 = t1 - tlight;

    if (abs(t3 - t2) > tol)

        t2 = t3;

    else

        break

    end

end


function outpoints = icbm_spm2tal(inpoints)
%
% This function converts coordinates from MNI space (normalized
% using the SPM software package) to Talairach space using the
% icbm2tal transform developed and validated by Jack Lancaster
% at the Research Imaging Center in San Antonio, Texas.
%
% http://www3.interscience.wiley.com/cgi-bin/abstract/114104479/ABSTRACT
%
% FORMAT outpoints = icbm_spm2tal(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
% (N being the number of points)
%
% ric.uthscsa.edu 3/14/07

% find which dimensions are of size 3
dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  error('input must be a N by 3 or 3 by N matrix')
end

% 3x3 matrices are ambiguous
% default to coordinates within a row
if dimdim == [1 2]
  disp('input is an ambiguous 3 by 3 matrix')
  disp('assuming coordinates are row vectors')
  dimdim = 2;
end

% transpose if necessary
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different for each software package
icbm_spm = [0.9254 0.0024 -0.0118 -1.0207
	   	   -0.0048 0.9316 -0.0871 -1.7667
            0.0152 0.0883  0.8924  4.0926
            0.0000 0.0000  0.0000  1.0000];

% apply the transformation matrix
inpoints = [inpoints; ones(1, size(inpoints, 2))];
inpoints = icbm_spm * inpoints;

% format the outpoints, transpose if necessary
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


function X = ttm(X,V,varargin)
%TTM Tensor times matrix for ktensor.
%
%   Y = TTM(X,A,N) computes the n-mode product of the ktensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be a
%   ktensor of the same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of the ktensor
%   X with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = ktensor({rand(5,2),rand(3,2),rand(4,2),rand(2,2)});
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also KTENSOR, KTENSOR/TTV, TENSOR/TTM.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

% Check for transpose option
isTranspose = false;
if numel(varargin) > 0
  if isnumeric(varargin{1});
    dims = varargin{1};
  end
  isTranspose =  (ischar(varargin{end}) && (varargin{end} == 't'));
end

% Check for dims argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with V as a
% cell array with one element.
if ~iscell(V)
    X = ttm(X,{V},dims,varargin{end});
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));

% Determine correct size index
if isTranspose
  j = 1;
else
  j = 2;
end

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if (ndims(V) ~= 2) || (size(V{vidx(i)},j) ~= size(X,dims(i)))
disp(size(V{vidx(i)}))
disp(size(X))

        error('Multiplicand is wrong size');
    end
end

% Do the multiplications in the specified modes.
for i = 1:numel(dims)
  if isTranspose
    X.u{dims(i)} = V{vidx(i)}'* X.u{dims(i)};
  else
    X.u{dims(i)} = V{vidx(i)} * X.u{dims(i)};
  end
end


function [xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)

%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil
%Version 1.20
%Created by Stepen (zerocross_raptor@yahoo.com)
%Created 20 November 2010
%Last modified 30 November 2011
%--------------------------------------------------------------------------
%GenerateNACASeries5Airfoil generates the airfoil vertexes' coordinate of
%the given NACA Series 5 Airfoil. The math equation used to generates the
%airfoil coordinates is based on Theory of Wing Section Chapter 6 by Abbott
%and Doenhoff.
%--------------------------------------------------------------------------
%Syntax:
%[xu,yu,xl,yl,xc,yc]=GenerateNACASeries5Airfoil(afid,dotcount,xmod)
%Input argument:
%- afid (1 x 5 str) specifies NACA Series 5 Airfoil identifier.
%- dotcount (1 x 1 int) specifies the number of vertexes to be generated on
%  the airfoil's camber/mean line.
%- xmod (str) specifies the mode of airfoil vertex distribution. Enter
%  'Uniform' to create uniform vertex distribution or 'Cosine' to create
%  vertex distribution based on cosine function (More vertex on the leading
%  edge region).
%Output argument:
%- xu (i x 1 num) specifies the x axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of xu) to the trailing
%  edge (the last element of xu).
%- yu (i x 1 num) specifies the y axis location of airfoil's upper surface
%  vertexes in fraction of chord. The airfoil's upper surface vertex are
%  arranged from leading edge (the first element of yu) to the trailing
%  edge (the last element of yu).
%- xl (i x 1 num) specifies the x axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of xl) to the trailing
%  edge (the last element of xl).
%- yl (i x 1 num) specifies the y axis location of airfoil's lower surface
%  vertexes in fraction of chord. The airfoil's lower surface vertex are
%  arranged from leading edge (the first element of yl) to the trailing
%  edge (the last element of yl).
%- xc (i x 1 num) specifies the x axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of xc) to the trailing
%  edge (the last element of xc).
%- yc (i x 1 num) specifies the y axis location of airfoil's camber line
%  vertexes in fraction of chord. The airfoil's camber line vertex are
%  arranged from leading edge (the first element of yc) to the trailing
%  edge (the last element of yc).
%--------------------------------------------------------------------------

%CodeStart-----------------------------------------------------------------
%Checking input afid
    if ~ischar(afid)
        error('Airfoil identifier must be a string!')
    end
    if numel(afid)~=5
        error('Airfoil identifier must be a 5 digit number!')
    end
    if isempty(str2double(afid))
        error('Airfoil identifier must be a 5 digit number!')
    end
    id=str2double(afid([2,3]));
    if (id~=10)&&(id~=20)&&(id~=30)&&(id~=40)&&(id~=50)
        error(['Airfoil identifier must start with X10,',...
               ' X20, X30, X40, or X50!'])
    end
%Checking input dotcount
    if numel(dotcount)~=1
        error('Number of vertex must be scalar!')
    end
    if (mod(dotcount,1~=0))||(dotcount<0)
        error('Number of vertex must be positive integer!')
    end
%Checking input xmod
    if nargin<3
        xmod='Cosine';
    end
    if (~strcmpi(xmod,'Uniform'))&&(~strcmpi(xmod,'Cosine'))
        error('Vertex distribution input must be Uniform or Cosine!')
    end
%Declaring look-up table
    mtable=[0.0580,0.1260,0.2025,0.2900,0.3910];
    ktable=[361.4000,51.6400,15.9570,6.6430,3.2300];
%Assigning identifier to equation coefficient
    id1=str2double(afid(1));
    id2=str2double(afid([2,3]));
    id3=str2double(afid([4,5]));
    cl=id1*(3/2)*(10/100);      %Design lift coefficient
    p=id2*(1/2)*(1/100);        %Maximum camber location
    t=id3*(1/100);              %Maximum thickness
    if t==0
        warning(['Zero thickness airfoil!',...
                 ' Airfoil will be just a camber line!'])
    end
    m=mtable(str2double(afid(2)));
    k=ktable(str2double(afid(2)));
%Calculating x-axis location of camber line vertexes
    panelcount=dotcount-1;
    if strcmpi(xmod,'Uniform')
        panellength=1/panelcount;
        xc=(0:panellength:1)';
    elseif strcmpi(xmod,'Cosine')
        deltadeg=90/panelcount;
        xc=1-cosd(0:deltadeg:90)';
    end
%Preallocating array for speed
    yc=zeros(dotcount,1);
    gc=zeros(dotcount,1);
    yt=zeros(dotcount,1);
    xu=zeros(dotcount,1);
    xl=zeros(dotcount,1);
    yu=zeros(dotcount,1);
    yl=zeros(dotcount,1);
%Calculating y-axis location of camber line vertexes and camber gradient
    if m~=0
        for i=1:1:dotcount
            if xc(i)<=p
                yc(i)=(k/6)*((xc(i)^3)-...
                             (3*m*(xc(i)^2))+...
                             ((m^2)*(3-m)*xc(i)));
                gc(i)=(k/6)*((2*(xc(i)^2))-...
                             (6*m*xc(i))+...
                             (((3*(m^2))-(m^3))));
            elseif xc(i)>p
                yc(i)=(k/6)*(m^3)*(1-xc(i));
                gc(i)=-1*(k/6)*(m^3);
            end
        end
    end
%Correcting camber line vertexes and camber gradient for extended series
    yc=yc*(id1/2);
    gc=gc*(id1/2);
%Converting camber gradient to camber slope and normal
    sc=atand(gc);
%Calculating thickness distribution
    for i=1:1:dotcount
        yt(i)=5*t*((0.29690*(xc(i)^0.5))-...
                   (0.12600*xc(i))-...
                   (0.35160*(xc(i)^2))+...
                   (0.28430*(xc(i)^3))-...
                   (0.10150*(xc(i)^4)));
    end
%Generating airfoil vertexes
    for i=1:1:dotcount
        xu(i)=xc(i)-yt(i)*sind(sc(i));
        yu(i)=yc(i)+yt(i)*cosd(sc(i));
        xl(i)=xc(i)+yt(i)*sind(sc(i));
        yl(i)=yc(i)-yt(i)*cosd(sc(i));
    end
%CodeEnd-------------------------------------------------------------------

end


function [Vout,HT_index, HT_values]=make_halfway_vertices(EV_table,ET_table,ETV_index,V,Ne)
% ETV_index_sparse=sparse(size(V,1),size(V,1));
%
% for i=1:size(ETV_index,1)
% 	if(ETV_index(i,1)>0)
% 		ETV_index_sparse(ETV_index(i,1),ETV_index(i,2))=i;
% 	else
% 		ET_table=ET_table(1:(i-1),:);
% 		EV_table=EV_table(1:(i-1));
% 		break
% 	end
% end

% Table to cell array
ETV_index_vall=cell(length(V),1);
for i=1:size(ETV_index,1)
    if(ETV_index(i,1)>0)
        ETV_index_vall{ETV_index(i,1)}=[ETV_index_vall{ETV_index(i,1)} i];
    else
        ET_table=ET_table(1:(i-1),:);
        EV_table=EV_table(1:(i-1));
        break;
    end
end

HT_index=cell(length(V),1);
HT_values=cell(length(V),1);

% Make output V
Vout=zeros(size(V,1)*4,3);
Vout(1:size(V,1),:)=V;
Vindex=size(V,1);

for i=1:length(V)
    Pneig=Ne{i};
    for j=1:length(Pneig);
        % Get the tangent and velocity of the edge P -> Pneig
        index=Ne{i}; vals=ETV_index_vall{i}; select1=vals(index==Pneig(j));
        Va=EV_table( select1); Ea=ET_table( select1,:);
        % Get the tangent and velocity of the edge Pneig -> P
        index=Ne{Pneig(j)}; vals=ETV_index_vall{Pneig(j)}; select2=vals(index==i);
        Vb=EV_table(select2); Eb=ET_table(select2,:);

        % The four points describing the spline
        P0=V(i,:);
        P3=V(Pneig(j),:);
        P1=P0+Ea*Va/3;
        P2=P3+Eb*Vb/3;

        % Spline used to calculated the xyz coordinate of the middle of each edge;
        c = 3*(P1 - P0);
        b = 3*(P2 - P1) - c;
        a = P3 - P0 - c - b;

        halfwayp = a*0.125 + b*0.250 + c*0.500 + P0;

        % Save the edge middle point
        if(sum(HT_index{i}==Pneig(j))==0)
            Vindex=Vindex+1;
            Vout(Vindex,:)=halfwayp;
            HT_index {i}= [HT_index{i} Pneig(j)];
            HT_values{i}=[HT_values{i} Vindex];
            HT_index {Pneig(j)}=[HT_index{ Pneig(j)} i];
            HT_values{Pneig(j)}=[HT_values{Pneig(j)} Vindex];
        end
    end
end
Vout=Vout(1:Vindex,:);


function newFaces = minConvexHull(points, varargin)
%MINCONVEXHULL Return the unique minimal convex hull of a set of 3D points
%
%   FACES = minConvexHull(PTS)
%   NODES is a set of 3D points  (as a Nx3 array). The function computes
%   the convex hull, and merge contiguous coplanar faces. The result is a
%   set of polygonal faces, such that there are no coplanar faces.
%   FACES is a cell array, each cell containing the vector of indices of
%   nodes given in NODES for the corresponding face.
%
%   FACES = minConvexHull(PTS, PRECISION)
%   Adjust the threshold for deciding if two faces are coplanar or
%   parallel. Default value is 1e-14.
%
%   Example
%     % extract square faces from a cube
%     [n, e, f] = createCube;
%     f2 = minConvexHull(n);
%     drawMesh(n, f2);
%
%     % Subdivides and smooths a mesh rpresenting a cube
%     [n, e, f] = createCube;
%     [n2, f2] = subdivideMesh(n, triangulateFaces(f), 4);
%     [n3, f3] = smoothMesh(n2, f2);
%     figure; drawMesh(n3, f3);
%     axis equal; view(3);
%     % merge coplanar faces, making apparent the faces of the original cube
%     f4 = minConvexHull(n3);
%     figure; drawMesh(n3, f4);
%     axis equal; view(3);
%
%
%   See also
%   meshes3d, mergeCoplanarFaces, drawMesh, convhull, convhulln
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2006-07-05
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

% HISTORY
%   20/07/2006 add tolerance for coplanarity test
%   21/08/2006 fix small bug due to difference of methods to test
%       coplanarity, sometimes resulting in 3 points of a face being not
%       coplanar! Also add control on precision
%   18/09/2007 ensure faces are given as horizontal vectors

% set up precision
acc = 1e-14;
if ~isempty(varargin)
    acc = varargin{1};
end

% triangulated convex hull. It is not uniquely defined.
faces = convhulln(points);

% compute centroid of the nodes
pointsCentroid = centroid(points);

% number of base triangular faces
N = size(faces, 1);

% compute normals of given faces
normals = planeNormal(createPlane(...
    points(faces(:,1),:), points(faces(:,2),:), points(faces(:,3),:)));

% initialize empty faces
newFaces = {};

% Processing flag for each triangle
% 1 : triangle to process, 0 : already processed
% in the beginning, every triangle face need to be processed
flag = ones(N, 1);

% iterate on each triangular face of the convex hull
for iFace = 1:N

    % check if face was already performed
    if ~flag(iFace)
        continue;
    end

    % indices of faces with same normal
    ind = find(abs(vectorNorm3d(cross(repmat(normals(iFace, :), [N 1]), normals)))<acc);
    ind = ind(ind~=iFace);

    % keep only coplanar faces (test coplanarity of points in both face)
    ind2 = iFace;
    for j = 1:length(ind)
        if isCoplanar(points([faces(iFace,:) faces(ind(j),:)], :), acc)
            ind2 = [ind2 ind(j)]; %#ok<AGROW>
        end
    end

    % compute order of the vertices in current face
    faceVertices = unique(faces(ind2, :));
    [tmp, I]  = angleSort3d(points(faceVertices, :)); %#ok<ASGLU>

    % create the new face, ensuring it is a row vector
    face = faceVertices(I);
    face = face(:)';

    % ensure face has normal pointing outwards
    outerNormal = meshFaceCentroids(points, face) - pointsCentroid;
    if dot(meshFaceNormals(points, face), outerNormal, 2) < 0
        face = face([1 end:-1:2]);
    end

    % add a new face to the list
    newFaces = [newFaces {face}]; %#ok<AGROW>

    % mark processed faces
    flag(ind2) = 0;
end


%EXnmr_cgls_mrnsd Example script, 2D NMR relaxometry

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% Clear workspace and command window.
clear, clc

% Choose if you would like to see the results displayed in a single figure
% window ('subplots') or in multiple figure windows ('manyplots').
dispres = 'subplots';
% dispres = 'manyplots';

LW = 2;  % Plot line width.
MS = 10; % Size of markers on plots.

rng(0);  % Make sure this test is repeatable.

% Define the test problem.
n = 64;
NoiseLevel = 0.05;
[A, b, x, ProbInfo] = PRnmr(n);
bn = PRnoise(b, NoiseLevel);

% Compute a CGLS solution, specifying the noise level for the discrepancy
% principle, and use true solution to compute error norms.
eta = 1.02;
options = IRset('x_true', x, 'NoiseLevel', NoiseLevel, 'eta', eta, 'NoStop', 'on');
[x_cgls, IterInfo_cgls] = IRcgls(A, bn, 1:500, options);

% Compute MRNSD reconstruction.
K = [1, 100:100:20000];
[x_mrnsd, IterInfo_mrnsd] = IRmrnsd(A, bn, K, options);

% Display the reconstructions;
% uncomment as appropriate to avoid displaying titles and legends.
if strcmp(dispres, 'subplots')
    figure(1), clf
    subplot(3,3,1), PRshowx(x, ProbInfo), colormap hsv
    title('True solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,4), PRshowx(IterInfo_cgls.BestReg.X, ProbInfo), colormap hsv
    title('Best CGLS solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,2), semilogy(IterInfo_mrnsd.Enrm,'linewidth',1.5)
    hold on
    hl = legend('{\tt info.Enrm}');
    % hl = legend('IRmrnsd error');
    set(hl,'interpreter','latex','fontsize',12)
    semilogy(IterInfo_mrnsd.BestReg.It, IterInfo_mrnsd.BestReg.Enrm, 'ro', 'LineWidth', 1.5, 'MarkerSize', 6)
    axis([0 max(K) 0.08 1.2])
    semilogy(IterInfo_mrnsd.StopReg.It, IterInfo_mrnsd.StopReg.Enrm, 'ms', 'LineWidth', 1.5, 'MarkerSize', 6)
    set(gca,'fontsize',12)
    title('Error history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,5), PRshowx(IterInfo_mrnsd.BestReg.X, ProbInfo)
    title(['Best MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.BestReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,3)
    semilogy(K,IterInfo_mrnsd.Rnrm(K),'-',K,eta*NoiseLevel*ones(size(K)),'--','linewidth',1.5)
    hl = legend('{\tt info.Rnrm}','{\tt eta*NoiseLevel}','location','northwest');
    % hl = legend('IRmrnsd residual','{\tt eta*NoiseLevel}','location','northwest');
    set(hl,'interpreter','latex')
    axis([0 max(K) 0.045 0.08])
    title('Residula history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
    %
    subplot(3,3,6), PRshowx(IterInfo_mrnsd.StopReg.X, ProbInfo)
    title(['DP MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.StopReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',10)
elseif strcmp(dispres, 'manyplots')
    figure(1), clf
    PRshowx(x,ProbInfo), colormap hsv
    title('True solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(2), clf
    PRshowx(IterInfo_cgls.BestReg.X, ProbInfo), colormap hsv
    title('Best CGLS solution','interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(3), clf
    semilogy(IterInfo_mrnsd.Enrm,'linewidth',LW)
    hold on
    semilogy(IterInfo_mrnsd.BestReg.It, IterInfo_mrnsd.BestReg.Enrm, 'ro', 'LineWidth', LW, 'MarkerSize', MS)
    semilogy(IterInfo_mrnsd.StopReg.It, IterInfo_mrnsd.StopReg.Enrm, 'ms', 'LineWidth', LW, 'MarkerSize', MS)
    axis([0 max(K) 0.08 1.2])
    % hl = legend('{\tt info.Enrm}','location','North');
    % set(hl,'interpreter','latex','fontsize',24)
    hl = legend('IRmrnsd error','optimal stopping iteration', ...
      'DP stopping iteration','location','North');
    set(hl,'interpreter','latex','fontsize',16)
    title('Error history','interpreter','latex','fontsize',24)
    set(gca,'fontsize',30)
    %
    figure(4), clf
    PRshowx(IterInfo_mrnsd.BestReg.X, ProbInfo), colormap hsv
    title(['Best MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.BestReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
    %
    figure(5), clf
    semilogy(K,IterInfo_mrnsd.Rnrm(K),'-',K,eta*NoiseLevel*ones(size(K)),'--','linewidth',LW)
    % hl = legend('{\tt info.Rnrm}','{\tt eta*NoiseLevel}','location','north');
    hl = legend('IRmrnsd residual','{\tt eta*NoiseLevel}','location','north');
    set(hl,'interpreter','latex','fontsize',24)
    title('Residual history','interpreter','latex','fontsize',24)
    axis([0 max(K) 0.045 0.08])
    set(gca,'fontsize',30)
    %
    figure(6), clf
    PRshowx(IterInfo_mrnsd.StopReg.X,ProbInfo), colormap hsv
    title(['DP MRNSD sol., $k$ = ',num2str(IterInfo_mrnsd.StopReg.It)],...
    'interpreter','latex','fontsize',24)
    set(gca,'fontsize',24)
end

return

% A number of instructions useful to save the displayed figures follow;
% the defualt is not to execute them. If you wish to save the displayed
% figures in the dedicated 'Results' folder, please comment the above
% return statement
oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), print -dpng -r300 EXnmr
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    % save as eps
    figure(1), print -depsc -r300 EXnmr_a.eps
    figure(2), print -depsc -r300 EXnmr_d.eps
    figure(3), print -depsc -r300 EXnmr_b.eps
    figure(4), print -depsc -r300 EXnmr_e.eps
    figure(5), print -depsc -r300 EXnmr_c.eps
    figure(6), print -depsc -r300 EXnmr_f.eps
    % save as png
    figure(1), print -dpng -r300 EXnmr_a
    figure(2), print -dpng -r300 EXnmr_d
    figure(3), print -dpng -r300 EXnmr_b
    figure(4), print -dpng -r300 EXnmr_e
    figure(5), print -dpng -r300 EXnmr_c
    figure(6), print -dpng -r300 EXnmr_f
end
cd(oldcd)

% Uncomment the following return statement if you wish to save the
% displayed figures as MATLAB figures

% return

oldcd = cd;
if strcmp(dispres, 'subplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    figure(1), saveas('EXnmr.fig')
elseif strcmp(dispres, 'manyplots')
    try
        cd('Results')
    catch
        mkdir('Results')
        cd('Results')
    end
    saveas(figure(1), 'EXnmr_a.fig')
    saveas(figure(2), 'EXnmr_b.fig')
    saveas(figure(3), 'EXnmr_c.fig')
    saveas(figure(4), 'EXnmr_d.fig')
    saveas(figure(5), 'EXnmr_e.fig')
    saveas(figure(6), 'EXnmr_f.fig')
end
cd(oldcd)


classdef RMMEDA_F5 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing RM-MEDA

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, and Y. Jin, RM-MEDA: A regularity model-based
% multiobjective estimation of distribution algorithm, IEEE Transactions on
% Evolutionary Computation, 2008, 12(1): 41-63.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            g = 1 + 9*mean((X(:,2:end).^2-repmat(X(:,1),1,size(X,2)-1)).^2,2);
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1)./g));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function suborder = triangle_nco_suborder ( rule, suborder_num )

%*****************************************************************************80
%
%% TRIANGLE_NCO_SUBORDER returns the suborders for an NCO rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer SUBORDER_NUM, the number of suborders of the rule.
%
%    Output, integer SUBORDER(SUBORDER_NUM), the suborders of the rule.
%
  if ( rule == 1 )
    suborder(1:suborder_num) = [ ...
      1 ];
  elseif ( rule == 2 )
    suborder(1:suborder_num) = [ ...
      3 ];
  elseif ( rule == 3 )
    suborder(1:suborder_num) = [ ...
      3, 3 ];
  elseif ( rule == 4 )
    suborder(1:suborder_num) = [ ...
      3, 6, 1 ];
  elseif ( rule == 5 )
    suborder(1:suborder_num) = [ ...
      3, 6, 3, 3 ];
  elseif ( rule == 6 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 3 ];
  elseif ( rule == 7 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 3, 6, 1 ];
  elseif ( rule == 8 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 6, 6, 3, 3 ];
  elseif ( rule == 9 )
    suborder(1:suborder_num) = [ ...
      3, 6, 6, 3, 6, 6, 3, 6, 3, 3  ];

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGLE_NCO_SUBORDER - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'TRIANGLE_NCO_SUBORDER - Fatal error!' )

  end

  return
end


function smallpot = marginalize_pot(bigpot, keep, maximize, useC)
% MARGINALIZE_POT Marginalize a cpot onto a smaller domain.
% smallpot = marginalize_pot(bigpot, keep, maximize, useC)
%
% The maximize argument is ignored - maxing out a Gaussian is the same as summing it out,
% since the mode and mean are equal.
% The useC argument is ignored.

node_sizes = sparse(1, max(bigpot.domain));
node_sizes(bigpot.domain) = bigpot.sizes;
sum_over = mysetdiff(bigpot.domain, keep);

if sum(node_sizes(sum_over))==0 % isempty(sum_over)
  %smallpot = bigpot;
  smallpot = cpot(keep, node_sizes(keep), bigpot.g, bigpot.h, bigpot.K);
else
  [h1, h2, K11, K12, K21, K22] = partition_matrix_vec(bigpot.h, bigpot.K, sum_over, keep, node_sizes);
  n = length(h1);
  K11inv = inv(K11);
  g = bigpot.g + 0.5*(n*log(2*pi) - log(det(K11)) + h1'*K11inv*h1);
  if length(h2) > 0 % ~isempty(keep) % we are are actually keeping something
    A = K21*K11inv;
    h = h2 - A*h1;
    K = K22 - A*K12;
  else
    h = [];
    K = [];
  end
  smallpot = cpot(keep, node_sizes(keep), g, h, K);
end


function [y] = spm_gx_fmri_linear(x,u,P,M)
% Simulated BOLD response to input (linear version)
% FORMAT [y] = spm_gx_fmri_linear(x,u,P,M)
% y          - BOLD response (%)
% x          - state vector     (see spm_fx_fmri)
% P          - Parameter vector (see spm_fx_fmri)
% M          - model specification structure (see spm_nlsi)
%__________________________________________________________________________
%
% This function implements the BOLD signal model described in:
%
% Stephan KE, Weiskopf N, Drysdale PM, Robinson PA, Friston KJ (2007)
% Comparing hemodynamic models with DCM. NeuroImage 38: 387-401.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston & Klaas Enno Stephan
% $Id: spm_gx_fmri_linear.m 6262 2014-11-17 13:47:56Z karl $

% Biophysical constants for 1.5T
%==========================================================================

% time to echo (TE) (default 0.04 sec)
%--------------------------------------------------------------------------
try, TE = M.TE; catch, TE = 0.04; end

% resting venous volume (%)
%--------------------------------------------------------------------------
V0  = 4;

% estimated region-specific ratios of intra- to extra-vascular signal
%--------------------------------------------------------------------------
ep  = 1*exp(P.epsilon);

% slope r0 of intravascular relaxation rate R_iv as a function of oxygen
% saturation S:  R_iv = r0*[(1 - S)-(1 - S0)] (Hz)
%--------------------------------------------------------------------------
r0  = 25;

% frequency offset at the outer surface of magnetized vessels (Hz)
%--------------------------------------------------------------------------
nu0 = 40.3;

% resting oxygen extraction fraction
%--------------------------------------------------------------------------
E0  = 0.4;

%-Coefficients in BOLD signal model
%==========================================================================
k1  = 4.3*nu0*E0*TE;
k2  = ep*r0*E0*TE;
k3  = 1 - ep;

%-Output equation of BOLD signal model
%==========================================================================
v   = x(:,4) + 1;
q   = x(:,5) + 1;
y   = V0*((k1 + k2).*(1 - q) + (k3 - k2).*(1 - v));


function cache = create_loc_cc_fftn_cache(metric, metric_param_pix, volmov, volfix, internal_dtype, maskfix, deps, loc_cc_approximate)
%     deps = 1e-5;
    Nd = 3;
    if size(volmov, 3) == 1
        Nd = 2;
    end
    szvolfix = [size(volfix, 1), size(volfix, 2), size(volfix, 3)];

    cache = [];
    if numel(metric_param_pix) == 1
        metric_param_pix = metric_param_pix * ones(1,Nd);
    end
    metric_param_pix = metric_param_pix(1:Nd);

    metr_threshold = 0.4;
    metr_threshold = 0.8;
    metric_param_pix(metric_param_pix < metr_threshold) = metr_threshold;
    sgm = metric_param_pix;
    hsz = max(2*ceil(2*sgm) + 1, 5);
    filterRadius = (hsz-1)/2;

    pad_size = ceil(filterRadius);
    start = pad_size+1;

    stop = start + size(volfix) - 1;
    vol_fix_p = padarray(volfix, pad_size, 'symmetric');

    if Nd == 2
        [n1, n2] = ndgrid(1:size(vol_fix_p,1), 1:size(vol_fix_p,2));
    elseif Nd == 3
        [n1, n2, n3] = ndgrid(1:size(vol_fix_p,1), 1:size(vol_fix_p,2), 1:size(vol_fix_p,3));
    end
    c = ceil(size(n1)/2)+1;
    if Nd == 2
        g = exp(- (n1-c(1)).^2/(2*sgm(1)^2) - (n2-c(2)).^2/(2*sgm(2)^2));
    elseif Nd == 3
        g = exp(- (n1-c(1)).^2/(2*sgm(1)^2) - (n2-c(2)).^2/(2*sgm(2)^2) - (n3-c(3)).^2/(2*sgm(3)^2));
    end

    g = g/sum(g(:));
    g = fftshift(g);
    fg = fftn(g, size(vol_fix_p));
    convop = @(fx) (ifftn(fftn(fx) .* fg, 'symmetric'));

    mean_fix = convop(vol_fix_p);
    sgm_fix = convop(vol_fix_p.^2) - mean_fix.^2 + deps;
    sgm_fix = sqrt(sgm_fix);

%     maskfix_pad = [];
    if ~isempty(maskfix)
        maskfix_pad = padarray(maskfix, pad_size, 'symmetric');
        sgm_fix = sgm_fix ./ maskfix_pad;
        sgm_fix_inv = maskfix_pad ./ sgm_fix;
    else
        sgm_fix_inv = 1 ./ sgm_fix;
    end

    cache.fg = fg;
    cache.vol_fix_p = vol_fix_p;
    cache.mean_fix = mean_fix;
    cache.sgm_fix = sgm_fix;
    cache.sgm_fix_inv = sgm_fix_inv;
    cache.start = start;
    cache.stop = stop;

    cache.pad_size = pad_size;
    cache.metric_name = metric;
    cache.sigma = metric_param_pix;
    cache.internal_dtype = internal_dtype;
    cache.deps = deps;
%     cache.maskfix = maskfix;
%     cache.maskfix_pad = maskfix_pad;
    cache.maskfix_pad = [];
    cache.loc_cc_approximate = loc_cc_approximate;

    if strcmp(metric, 'loc_cc_fftn_gpu')
        cache.fg = gpuArray((cache.fg));
        cache.vol_fix_p = gpuArray((cache.vol_fix_p));
        cache.mean_fix = gpuArray((cache.mean_fix));
        cache.sgm_fix = gpuArray((cache.sgm_fix));
        cache.deps = gpuArray(cache.deps);
        cache.sgm_fix_inv = gpuArray(sgm_fix_inv);
%         cache.maskfix = gpuArray(cache.maskfix);
%         cache.maskfix_pad = gpuArray(cache.maskfix_pad);
    elseif strcmp(metric, 'loc_cc_fftn_gpu_single')
        cache.fg = gpuArray(single(cache.fg));
        cache.vol_fix_p = gpuArray(single(cache.vol_fix_p));
        cache.mean_fix = gpuArray(single(cache.mean_fix));
        cache.sgm_fix = gpuArray(single(cache.sgm_fix));
        cache.deps = gpuArray(single(cache.deps));
        cache.sgm_fix_inv = gpuArray(single(sgm_fix_inv));
%         cache.maskfix = gpuArray(single(cache.maskfix));
%         cache.maskfix_pad = gpuArray(single(cache.maskfix_pad));
    elseif strcmp(metric, 'loc_cc_fftn_single')
        cache.fg = (single(cache.fg));
        cache.vol_fix_p = (single(cache.vol_fix_p));
        cache.mean_fix = (single(cache.mean_fix));
        cache.sgm_fix = (single(cache.sgm_fix));
        cache.deps = (single(cache.deps));
        cache.sgm_fix_inv = (single(sgm_fix_inv));
    end
end


function [f]=comp_idwilt(coef,g)
%COMP_IDWILT  Compute Inverse discrete Wilson transform.
%
%   This is a computational routine. Do not call it
%   directly.

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: OK
%   REFERENCE: OK

M=size(coef,1)/2;
N=2*size(coef,2);
W=size(coef,3);

a=M;

L=N*a;

coef2=zeros(2*M,N,W,assert_classname(coef,g));

% First and middle modulation are transferred unchanged.
coef2(1,1:2:N,:) = coef(1,:,:);
if mod(M,2)==0
  coef2(M+1,1:2:N,:) = coef(M+1,:,:);
else
  coef2(M+1,2:2:N,:) = coef(M+1,:,:);
end;

if M>2
  % cosine, first column.
  coef2(3:2:M,1:2:N,:)        = 1/sqrt(2)*coef(3:2:M,:,:);
  coef2(2*M-1:-2:M+2,1:2:N,:) = 1/sqrt(2)*coef(3:2:M,:,:);

  % sine, second column
  coef2(3:2:M,2:2:N,:)        = -1/sqrt(2)*i*coef(M+3:2:2*M,:,:);
  coef2(2*M-1:-2:M+2,2:2:N,:) =  1/sqrt(2)*i*coef(M+3:2:2*M,:,:);
end;

% sine, first column.
coef2(2:2:M,1:2:N,:)        = -1/sqrt(2)*i*coef(2:2:M,:,:);
coef2(2*M:-2:M+2,1:2:N,:)   =  1/sqrt(2)*i*coef(2:2:M,:,:);

% cosine, second column
coef2(2:2:M,2:2:N,:)        = 1/sqrt(2)*coef(M+2:2:2*M,:,:);
coef2(2*M:-2:M+2,2:2:N,:)   = 1/sqrt(2)*coef(M+2:2:2*M,:,:);

f = comp_isepdgt(coef2,g,L,a,2*M,0);

% Apply the final DGT
%f=comp_idgt(coef2,g,a,[0 1],0,0);

% Clean signal if it is known to be real
if (isreal(coef) && isreal(g))
  f=real(f);
end;


% SP_EXTERIOR_DERIVAITVE: computes the exterior derivative as a matrix with size
%  given by the dimension of two consecutive spaces in the De Rham sequence.
%
%   diff_op = sp_exterior_derivative (space1, space2);
%
% INPUT:
%
%   space1:  domain space of the exterior derivative (number of columns)
%   space2:  image space of the exterior derivative (number of rows)
%
% OUTPUT:
%
%   diff_op: sparse matrix representation of the differential operator.
%
% Copyright (C) 2020-2023 Bernard Kapidani, Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function diff_op = sp_exterior_derivative (space1, space2)

  assert (strcmpi(space1.transform, 'grad-preserving'), ...
    'The first space cannot be the one for integral-preserving splines (or n-forms)')

  ndim = numel (space1.knots);
  if (ndim == 1)
    grad_curl = 'grad';
    assert (space1.degree == space2.degree+1, 'The degrees are not compatible')
    assert (numel(space1.knots{1}) == numel(space2.knots{1})+2, 'The knot vectors are not compatible')
  elseif (ndim == 2)
    if (strcmpi (space2.transform, 'curl-preserving'))
      grad_curl = 'grad';
      deg_shift = {[1 0], [0 1]};
      knt_shift = {[2 0], [0 2]};
    elseif (strcmpi (space2.transform, 'div-preserving'))
      grad_curl = 'curl';
      deg_shift = {[0 1], [1 0]};
      knt_shift = {[0 2], [2 0]};
    else
      error ('The second space should be either curl-preserving or div-preserving')
    end
    for idim = 1:ndim
      assert (all(space1.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  elseif (ndim == 3)
    grad_curl = 'grad';
    deg_shift = {[1 0 0], [0 1 0], [0 0 1]};
    knt_shift = {[2 0 0], [0 2 0], [0 0 2]};
    assert (strcmpi(space2.transform, 'curl-preserving'), ...
      'The second space should be the one for curl-conforming splines')
    for idim = 1:ndim
      assert (all(space1.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  end

  diff_ops = op_geom_exterior (space1.knots, space1.degree, grad_curl);
  diff_op = diff_ops{1};
end


function [d,S] = solve_for_d_S(Y,A,R,options)
[N,B] = size(Y);
M = size(A,2);

sigma0 = 0.1; % initial sigma
sigma_max = 1;
sigma_min = 1e-9;
delta_t0 = 1e-9;

disp('Start estimating D and S');
C = kron((A'*A),eye(B));
YAR = (Y-A*R)';
Z = YAR*A;
E = diag(sum(YAR.^2,2));
F1 = YAR*A;
F = zeros(M*B,B);
for i = 1:M
    F((i-1)*B+1:i*B,:) = diag(F1(:,i));
end

d = (1/N)*sum(YAR.^2,2);
d = 1./sqrt(d);

S = eye(M*B,M*B);
if 0
    u = 1/sqrt(B) * ones(B,1);
    S1 = sigma0^2*u*u' + 1e-4*eye(B);
    S1 = inv(S1);
else
    S1 = sigma0^(-2)*eye(B);
end

for j = 1:M
    inds = (j-1)*B+1:j*B;
    S(inds,inds) = diag(1./d) * S1 * diag(1./d);
end

errors2 = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N);
for iter = 1:300
    % solve for S
    z = diag(d)*Z;
    z = z(:);
    if 0
        obj_fun_S = @(S) eval_obj_fun_S(S,C,z,M,B);
        fun_der_S = @(S) calc_der_S(S,C,z,M,B);
        proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,M,B);

        S = projected_gradient_descent(S, obj_fun_S, fun_der_S, ...
            proj_fun_S, delta_t0);
    else
        if 1
            proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,1,B);
            for j = 1:M
                obj_fun_Sj = @(Sj) eval_obj_fun_Sj(Sj,S,C,z,M,B,j);
                fun_der_Sj = @(Sj) calc_der_Sj(Sj,S,C,z,B,j);
                inds = (j-1)*B+1:j*B;
                Sj = S(inds,inds);
                Sj = projected_gradient_descent(Sj, obj_fun_Sj, fun_der_Sj, ...
                    proj_fun_S, 1e-5);
                S(inds,inds) = Sj;
            end
        else
            der_S = calc_der_S(S,C,z,M,B);
            proj_fun_S = @(S) project_to_spd(S,d,sigma_min,sigma_max,1,B);
            for j = 1:M
                obj_fun_Sj = @(Sj) eval_obj_fun_Sj(Sj,S,C,z,M,B,j);
                fun_der_Sj = @(Sj) calc_der_Sj_approx(Sj,der_S,B,j);
                inds = (j-1)*B+1:j*B;
                Sj = S(inds,inds);
                Sj = projected_gradient_descent(Sj, obj_fun_Sj, fun_der_Sj, ...
                    proj_fun_S, 1e-3);
                S(inds,inds) = Sj;
            end
        end
    end

    % solve for D
    Q = S + C;
    G = (1/N)*(E-F'*(Q\F));
    fcn_d = @(d) G*d - 1./d;
    fsolve_opts = optimset('Display','off');
    d = fsolve(fcn_d, 1./sqrt((1/N)*diag(E)), fsolve_opts);

    % calc error
    err = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N);
    errors2 = [errors2;err];
    if test_convergence(errors2, 1e-8), break; end

    if mod(iter,10) == 0
        disp(['Process iteration ',num2str(iter),'. The objective ',...
            'function has value ',num2str(err)]);
    end
end

function S = project_to_spd(S,d,sigma_min,sigma_max,M,B)
touch_bd = 0;
S = (S + S')/2;
for j = 1:M
    inds = (j-1)*B+1:j*B;
    S3 = S(inds,inds);
    SigmaInv = diag(d) * S3 * diag(d);
    [V,D] = eig(SigmaInv);
    d1 = diag(D);
    low_bd = 1/(sigma_max^2);
    up_bd = 1/(sigma_min^2);

    if ~isempty(find(d1<low_bd, 1))
        touch_bd = touch_bd + 1;
%         break;
    end

    d1(d1<low_bd) = low_bd;
    d1(d1>up_bd) = up_bd;
    S(inds,inds) = diag(1./d)*V*diag(d1)*V'*diag(1./d);
end

if touch_bd >= 1
%     disp(['Uncertainty amount upper bound touched ',num2str(touch_bd),' times']);

    % Once uncertainty amount upper bound touched. It means the change of
    % the covariance is too large. Setting S to 0 will make the objective
    % function be Inf, thus revoke the delta_t.

%     if touch_bd >= 1
%         S = 0 * eye(M*B);
%     end
end

function val = eval_obj_fun_Sj(Sj,S,C,z,M,B,j)
inds = (j-1)*B+1:j*B;
S(inds,inds) = Sj;
val = eval_obj_fun_S(S,C,z,M,B);

function val = eval_obj_fun_S(S,C,z,M,B)
Q = S + C;
% R = chol(Q);
val1 = 0;
for j = 1:M
    inds = (j-1)*B+1:j*B;
    val1 = val1 + logdet(S(inds,inds));
end

% val = -z'*(Q\z) + logdet(Q) - val1;
R = chol(Q);
y = R'\z;
val = -sum(y.^2) + 2*sum(log(diag(R))) - val1;

function val = eval_obj_fun_D_S(S,d,C,YAR,Z,M,B,N)
z = diag(d)*Z;
z = z(:);

val = eval_obj_fun_S(S,C,z,M,B);
val = val + sum(sum((diag(d)*YAR).^2)) - 2*N*sum(log(d));

function der = calc_der_S(S,C,z,M,B)
Q = S + C;
invQ = inv(Q);
y = invQ*z;
T = y*y';
der = zeros(M*B,M*B);
for j = 1:M
    inds = (j-1)*B+1:j*B;
    Sj = S(inds,inds);
    der(inds,inds) = T(inds,inds) + invQ(inds,inds) - inv(Sj);
end

% der_norm = norm(der,'fro');
% der = der/der_norm;

function der = calc_der_Sj(Sj,S,C,z,B,j)
inds = (j-1)*B+1:j*B;
S(inds,inds) = Sj;

Q = S + C;
invQ = inv(Q);
y = invQ*z;
T = y*y';
der = zeros(B,B);

der = T(inds,inds) + invQ(inds,inds) - inv(Sj);

der_norm = norm(der,'fro');
der = der/der_norm;

function der = calc_der_Sj_approx(Sj,der_S,B,j)
inds = (j-1)*B+1:j*B;
der = der_S(inds,inds);

der_norm = norm(der,'fro');
der = der/der_norm;

function d = solve_for_d1(Y,A,R,S)
[N,B] = size(Y);
M = size(A,2);

YAR = Y - A*R;
E = diag(sum(YAR.^2,1));

Q = S + kron((A'*A),eye(B));

F1 = A'*YAR;
F = zeros(M*B,B);
for i = 1:M
    F((i-1)*B+1:i*B,:) = diag(F1(i,:));
end
G = (1/N)*(E-F'*(Q\F));

fcn_d = @(d) G*d - 1./d;
d = fsolve(fcn_d, 1./sqrt((1/N)*diag(E)));


function ef = cpf_qlim_event(cb_data, cx)
%CPF_QLIM_EVENT  Event function to detect gen reactive power limit violations
%   EF = CPF_QLIM_EVENT(CB_DATA, CX)
%
%   CPF event function to detect generator reactive power limit violations,
%   i.e. Qg <= Qmin or Qg >= Qmax.
%
%   Inputs:
%       CB_DATA : struct of data for callback functions
%       CX : struct containing info about current point (continuation soln)
%
%   Outputs:
%       EF : event function value

%   MATPOWER
%   Copyright (c) 2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%   and Shrirang Abhyankar, Argonne National Laboratory
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% event function value is 2 ng x 1 vector equal to:
%%      [ Qg - Qmax ]
%%      [ Qmin - Qg ]

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;

%% get updated MPC
d = cb_data;
mpc = cpf_current_mpc(d.mpc_base, d.mpc_target, ...
    d.Ybus, d.Yf, d.Yt, d.ref, d.pv, d.pq, cx.V, cx.lam, d.mpopt);

%% compute Qg violations for on-line gens, not at PQ buses
nb = size(mpc.bus, 1);
ng = size(mpc.gen, 1);
on = find(mpc.gen(:, GEN_STATUS) > 0 & ...  %% which generators are on?
          mpc.bus(mpc.gen(:, GEN_BUS), BUS_TYPE) ~= PQ);  %% ... and are not PQ buses
gbus = mpc.gen(on, GEN_BUS);                %% what buses are they at?
ngon = size(on, 1);

%% build connection matrix, element i, j is 1 if gen on(i) at bus j is ON
Cg = sparse((1:ngon)', gbus, ones(ngon, 1), ngon, nb);
C = Cg * Cg';

%% violations are based on total violation at bus, not individual violations
%% (see https://github.com/MATPOWER/matpower/issues/26)
v_Qmax = NaN(ng, 1);
v_Qmin = v_Qmax;
v_Qmax(on) = C * (mpc.gen(on, QG) - mpc.gen(on, QMAX));
v_Qmin(on) = C * (mpc.gen(on, QMIN) - mpc.gen(on, QG));

%% assemble event function value
ef = [v_Qmax; v_Qmin];


% [INPUT]
% data = A float t-by-n matrix containing the time series to be sanitized.
% x = A vector of length t containing the numeric observation dates of the time series (if empty, observations are assumed to be linearly spaced between 1 and t).
% w = An integer [5,21] representing the length of the moving window used to detect outliers (if empty, no outliers replacement is performed).
% m = A vector of 2 floats (-Inf,Inf) containing minimum and maximum clamping values (if empty, no clamping is performed).
%
% [OUTPUT]
% data = A float t-by-n matrix containing the sanitized time series.

function data = sanitize_data(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('data',@(x)validateattributes(x,{'double'},{'real' '2d' 'nonempty'}));
        ip.addRequired('x',@(x)validateattributes(x,{'double'},{'real'}));
        ip.addRequired('w',@(x)validateattributes(x,{'double'},{'real'}));
        ip.addRequired('m',@(x)validateattributes(x,{'double'},{'real'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    data = ipr.data;
    [x,w,m] = validate_input(data,ipr.x,ipr.w,ipr.m);

    nargoutchk(1,1);

    data = sanitize_data_internal(data,x,w,m);

end

function data = sanitize_data_internal(data,x,w,m)

    for i = 1:size(data,2)
        y = data(:,i);

        nan_indices = isnan(y);

        if (all(nan_indices))
            continue;
        end

        if (any(nan_indices))
            y = fill_missing_values(y,x,nan_indices);
        end

        if (~isempty(w))
            y = replace_outliers(y,x,w);
        end

        if (~isempty(m))
            y = min(max(y,m(1)),m(2));
        end

        data(:,i) = y;
    end

end

function y = fill_missing_values(y,x,nan_indices)

    d = diff(nan_indices);

    if (nan_indices(1))
        z = find(d == -1,1,'first');
        y(1:z) = y(z+1);
        nan_indices(1:z) = 0;
    end

    if (nan_indices(end))
        z = find(d == 1,1,'last');
        y(z+1:end) = y(z);
        nan_indices(z+1:end) = 0;
    end

    y(nan_indices) = spline(x(~nan_indices),y(~nan_indices),x(nan_indices));

end

function y = replace_outliers(y,x,w)

    f = 3 * (-1 / (sqrt(2) * erfcinv(1.5)));
    k = floor(w * 0.5);
    n = numel(y);
    p = nan(k,1);

    xp = [p; y; p];

    m_med = zeros(n,1);
    m_mad = zeros(n,1);

    for i = 1:k
        x_i = y(1:k+i);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    for i = k+1:n-k-1
        x_i = xp(i:i+w,:);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    for i = n-k:n
        x_i = y(i-k:end);
        m = median(x_i);

        m_med(i) = m;
        m_mad(i) = median(abs(x_i - m));
    end

    b = m_mad .* f;
    lb = m_med - b;
    ub = m_med + b;

    is_outlier = (y > ub) | (y < lb);
    d = diff(is_outlier);

    if (is_outlier(1) == 1)
        z = find(d == -1,1,'first');
        y(1:z) = y(z+1);
        is_outlier(1:z) = 0;
    end

    if (is_outlier(end))
        z = find(d == 1,1,'last');
        y(z+1:end) = y(z);
        is_outlier(z+1:end) = 0;
    end

    y(is_outlier) = spline(x(~is_outlier),y(~is_outlier),x(is_outlier));

end

function [x,w,m] = validate_input(data,x,w,m)

    t = size(data,1);

    if (isempty(x))
        x = 1:t;
    else
        if (~isvector(x))
            error('The value of ''x'' is invalid. Expected input to be a vector.');
        end

        if (numel(x) ~= t)
            error(['The value of ''x'' is invalid. Expected input to contain ' num2str(t) ' elements.']);
        end

        if (~all(isfinite(x)))
            error('The value of ''x'' is invalid. Expected input to contain finite elements.');
        end

        if (~all(diff(x) > 0))
            error('The value of ''x'' is invalid. Expected input to contain increasing elements.');
        end
    end

    if (~isempty(w))
        if (~isscalar(w))
            error('The value of ''w'' is invalid. Expected input to be a scalar.');
        end

        if (~isfinite(w))
            error('The value of ''w'' is invalid. Expected input to be finite.');
        end

        if (floor(w) ~= w)
            error('The value of ''w'' is invalid. Expected input to be an integer.');
        end

        if ((w < 5) || (w > 21))
            error('The value of ''w'' is invalid. Expected input to have a value >= 5 and <= 21.');
        end

        w = w - 1;
    end

    if (~isempty(m))
        if (~isvector(m))
            error('The value of ''m'' is invalid. Expected input to be a vector.');
        end

        if (numel(m) ~= 2)
            error('The value of ''m'' is invalid. Expected input to contain 2 elements.');
        end

        if (~all(isfinite(m)))
            error('The value of ''m'' is invalid. Expected input to contain finite elements.');
        end

        if (any(floor(m) ~= m))
            error('The value of ''m'' is invalid. Expected input to contain integer elements.');
        end

        if (m(1) >= m(2))
            error('The value of ''m'' is invalid. Expected input first element to be less than the input second element.');
        end
    end

end


%IMM_UPDATE  UKF based Interacting Multiple Model (IMM) Filter update step
%
% Syntax:
%   [X_i,P_i,MU,X,P] = IMM_UPDATE(X_p,P_p,c_j,ind,dims,Y,H,R)
%
% In:
%   X_p  - Cell array containing N^j x 1 mean state estimate vector for
%          each model j after prediction step
%   P_p  - Cell array containing N^j x N^j state covariance matrix for
%          each model j after prediction step
%   c_j  - Normalizing factors for mixing probabilities
%   ind  - Indices of state components for each model as a cell array
%   dims - Total number of different state components in the combined system
%   Y    - Dx1 measurement vector.
%   H    - Measurement matrices for each model as a cell array.
%   h    - Measurement mean
%   param - parameters
%   R    - Measurement noise covariances for each model as a cell array.
%
% Out:
%   X_i  - Updated state mean estimate for each model as a cell array
%   P_i  - Updated state covariance estimate for each model as a cell array
%   MU   - Probabilities of each model
%   X    - Combined state mean estimate
%   P    - Combined state covariance estimate
%
% Description:
%   IMM-UKF filter measurement update step. If some of the models have linear
%   measurements standard Kalman filter update step is used for those.
%
% See also:
%   IMM_PREDICT, IMM_SMOOTH, IMM_FILTER

% History:
%   01.11.2007 JH The first official version.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: imm_update.m 111 2007-11-01 12:09:23Z jmjharti $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

function [X_i,P_i,MU,X,P] = uimm_update(X_p,P_p,c_j,ind,dims,Y,H,h,R,param)
    % Number of models
    m = length(X_p);

    % Space for update state mean, covariance and likelihood of measurements
    X_i = cell(1,m);
    P_i = cell(1,m);
    lambda = zeros(1,m);

    % Update for each model
    for i = 1:m
        % Update the state estimates
        if isempty(h) | isempty(h{i})
            [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = kf_update(X_p{i},P_p{i},Y,H{i},R{i});
        else
            [X_i{i}, P_i{i}, K, IM, IS, lambda(i)] = ukf_update1(X_p{i},P_p{i},Y,h{i},R{i},param{i});
        end
    end

    % Calculate the model probabilities
    MU = zeros(1,m);
    c = sum(lambda.*c_j);
    MU = c_j.*lambda/c;

    % In case lambda's happen to be zero
    if c == 0
        MU = c_j;
    end

    % Output the combined updated state mean and covariance, if wanted.
    if nargout > 3
        % Space for estimates
        X = zeros(dims,1);
        P = zeros(dims,dims);
        % Updated state mean
        for i = 1:m
            X(ind{i}) = X(ind{i}) + MU(i)*X_i{i};
        end
        % Updated state covariance
        for i = 1:m
            P(ind{i},ind{i}) = P(ind{i},ind{i}) + MU(i)*(P_i{i} + (X_i{i}-X(ind{i}))*(X_i{i}-X(ind{i}))');
        end
    end


% BETALPR - Beta Distribution - Log Probability Ratio
% Copyright (c) 1998, Harvard University. Full copyright in the file Copyright
%
%   [ lpr ] = betalpr(p1,p2,alpha,beta)
%
% returns the log of the p(p1) / p(p2) when both
% are distributed Beta(alpha,beta).
%
% See also: METROP, *LPR

function [ lpr ] = betalpr(p1,p2,alpha,beta)
lpr = (alpha-1) * log(p1/p2) + (beta-1) * log((1-p1)/(1-p2)) ;


function f = comp_ifwt(c,g,a,J,Ls,ext)
%COMP_IFWT Compute Inverse DWT
%   Usage:  f = comp_ifwt(c,g,J,a,Ls,ext);
%
%   Input parameters:
%         c     : Cell array of length M = J*(filtNo-1)+1. Each element is Lc(m)*W array
%         g     : Synthesis wavelet filters - cell-array of length *filtNo*.
%         J     : Number of filterbank iterations.
%         a     : Upsampling factors - array of length *filtNo*.
%         Ls    : Length of the reconstructed signal.
%         ext   : 'per','zero','odd','even', Type of the forward transform boundary handling.
%
%   Output parameters:
%         f     : Reconstructed data - Ls*W array.
%

% see comp_fwt for explanantion
assert(a(1)==a(2),'First two elements of a are not equal. Such wavelet filterbank is not suported.');

% Impulse responses to a correct format.
filtNo = numel(g);
%gCell = cellfun(@(gEl) conj(flipud(gEl.h(:))),g,'UniformOutput',0);
gCell = cellfun(@(gEl) gEl.h(:),g,'UniformOutput',0);

if strcmp(ext,'per')
   % Initial shift of the filter to compensate for it's delay.
   % "Zero" delay reconstruction is produced.
   % offset = cellfun(@(gEl) gEl.offset,g);
   %offset = cellfun(@(gEl) 1-numel(gEl.h)-gEl.offset,g);
   offset = cellfun(@(gEl) gEl.offset,g);
elseif strcmp(ext,'valid')
   offset = -cellfun(@(gEl) numel(gEl.h)-1,g);
else
   % -1 + 1 = 0 is used for better readability and to be consistent
   % with the shift in comp_fwt.
   % Here we are cheating, because we are making the filters
   % anti-causal to compensate for the delay introduced by causal
   % analysis filters.
   % Instead, we could have used causal filters here and do the
   % delay compensation at the end (cropping f).
   % offset = -cellfun(@(gEl) numel(gEl),gCell) + (a -1) +1;
   offset = -(a-1);
end

Lc = cellfun(@(cEl) size(cEl,1),c);
Lc(end+1) = Ls;
tempca = c(1);
cRunPtr = 2;
for jj=1:J
   tempca=comp_ifilterbank_td([tempca;c(cRunPtr:cRunPtr+filtNo-2)],gCell,a,Lc(cRunPtr+filtNo-1),offset,ext);
   cRunPtr = cRunPtr + filtNo -1;
end
% Save reconstructed data.
f = tempca;

% for ch=1:chans
%   tempca = c(LcStart(1):LcEnd(1),ch);
%   LcRunPtr = filtNo+1;
%   cRunPtr = 2;
%   for jj=1:J
%      tempca = comp_upconv({tempca}, Lc(LcRunPtr),{tmpg{1}},a(1),skip(1),ext,0);
%      for ff=2:filtNo
%         % tempca = tempca + comp_upconv({c{cRunPtr}(:,ch)}, Lc(LcRunPtr),{tmpg},a(ff),skip,doNoExt,0);
%         tempca = tempca + comp_upconv({c(LcStart(cRunPtr):LcEnd(cRunPtr),ch)}, Lc(LcRunPtr),{tmpg{ff}},a(ff),skip(ff),ext,0);
%         cRunPtr = cRunPtr + 1;
%      end
%      LcRunPtr = LcRunPtr + filtNo -1;
%   end
%   f(:,ch) = tempca;
% end


function [M0, T1] = mtv_compute_m0_t1(data, flipAngles, TR, b1Map, roi, fixT1, verbose)

% function function [M0 T1] = mtv_compute_m0_t1 (data, flipAngles, TR [, b1Map, roi, fixT1, verbose])
% -----------------------------------------------------------
% This function performs a weighted-least squares data fit
% on SPGR T1-weighted data set
% INPUTS:
% data: width x length x slices x flipAngles matrix
% flipAngles: a vector of flip angles (in degrees) that corresponds to
% data(:,:,:,4)
% TR: in s
% b1Map: a width x length x slices matrix that contains the relative flip
% angle (i.e. if nominal alpha is 60, and measured alpha is 61, then b1Map
% = 61/60
% roi: binary mask (Matrix)
% fixT1: scalar or Matrix. If 0 --> do not fix T1.
% verbose: logical
%

if ndims(data)<3, data = permute(data(:),[2 3 4 1]); end
dataSize = size(data);
if (nargin < 4) || isempty(b1Map)
    b1Map = ones([dataSize(1:end-1) 1]);
end

if nargin<5 || isempty(roi)
    roi = ones(dataSize(1:end-1));
end

if nargin<6
    fixT1 = 0;
end

if nargin<7
    verbose = 0;
end

if max(size(b1Map) ~= dataSize(1:length(size(b1Map)))), error('B1 size is different from data size'); end

dims = size(data);
T1 = zeros(dims(1:end-1));
M0 = zeros(dims(1:end-1));

warning('off');
%sprintf('%s\n\n\n','loop over voxels...')
for vox=1:dims(1)*dims(2)*dims(3)
    %disp([sprintf('\b\b\b\b\b%3i',floor(vox/(dims(1)*dims(2)*dims(3))*100)) '%'])
    if roi(vox) && b1Map(vox)~=0

    kk=floor((vox-1)/(dims(1)*dims(2))); jj=floor((vox-kk*dims(1)*dims(2)-1)/(dims(1))); ii=vox-kk*dims(1)*dims(2)-jj*dims(1);
    kk=kk+1; jj=jj+1;
    %ii
        if ~fixT1
            %% T1 Mapping Using Variable Flip Angle SPGR Data With
            % Flip Angle Correction - Liberman, et al.
            y = squeeze(squeeze(data(ii,jj,kk, :)))./sin(flipAngles/180*pi*b1Map(vox))';
            x = squeeze(squeeze(data(ii,jj,kk, :)))./tan(flipAngles/180*pi*b1Map(vox))';

            % fit data
            param = polyfit(x,y,1);
%             fitresult = LinearFit(x, y, verbose);
%             param = coeffvalues(fitresult); % slope and intercept of the fitting
            param(isnan(param))=0;

%             ci = confint(fitresult,0.682); % confidence interval of the fitting (returns the slope and intercept of the lines framing the fit) -- corresponding to 2*sigma
%             ci(isnan(ci)) = 0;
%
            % compute PD and T1
            [~,T1(vox)]=getT1(param,TR);
        else
            %% if T1 is known
            T1(vox)=fixT1(min(vox,end));
        end
        %% Get M0
        [FA,iFA]=min(flipAngles);

        M0(vox)=getM0fromT1(T1(vox),TR(1),data(ii,jj,kk,iFA),FA*b1Map(vox));
        % add length of the confidence interval at 68.2% in the fourth
        % dimension

        if ~fixT1
%             [~,t1_min]=getT1(ci(1,:),TR);
%             [~,t1_max]=getT1(ci(2,:),TR);
%             M02(vox)=abs(getM0fromT1(t1_max,TR,data(ii,jj,kk,ialpha),alpha)-getM0fromT1(t1_min,TR,data(ii,jj,kk,ialpha),alpha));
%             T12(vox)=abs(t1_max-t1_min);
        end

    end
end
if ~fixT1
%    T1(:,:,:,2)=T12;
%     M0(:,:,:,2)=M02;
end
%display('...done')

function [fitresult, gof] = LinearFit(x, y, verbose)
%CREATEFIT(X,Y)
%  Create a fit.
%
%  Data for 'untitled fit 1' fit:
%      X Input : x
%      Y Output: y
%  Output:
%      fitresult : a fit object representing the fit.
%      gof : structure with goodness-of fit info.
%
%  See also FIT, CFIT, SFIT.

%  Auto-generated by MATLAB on 22-Jan-2015 15:51:35

% Fit: 'untitled fit 1'.
[xData, yData] = prepareCurveData( x, y );

% Set up fittype and options.
ft = fittype( 'poly1' );

% Fit model to data.
[fitresult, gof] = fit( xData, yData, ft );

% Plot fit with data.
if verbose && max(xData~=0) && max(yData~=0)

    figure(100)
    h = plot( fitresult, xData, yData,'+');
    set(h,'MarkerSize',30)
    legend( h, 'y vs. x', 'untitled fit 1', 'Location', 'NorthEast' );
    p11 = predint(fitresult,x,0.95,'observation','off');
    hold on
    plot(x,p11,'m--'); drawnow;
    hold off
    % Label axes
    xlabel( 'x' );
    ylabel( 'y' );
    grid on
    saveas(gcf,['temp.jpg']);
end

function [M0,T1]=getT1(param,TR)
a=param(1); % slope
b=param(2); % intercept
if a>0
    T1 = -TR/log(a);
else  % due to noise or bad fitting
    T1 = 0.000000000000001;
end
M0 = b/(1-exp(-TR/T1));

function M0=getM0fromT1(T1,TR,S,FA)
% Volz, S., N�th, U., Deichmann, R., 2012. Correction of systematic errors in quantitative proton density mapping. Magn. Reson. Med. 68, 74?85.
% Steady state
ST=(1-exp(-TR./T1))./(1-cos(FA*pi/180).*exp(-TR./T1)).*sin(FA*pi/180);
% M0=RP*PD (RP=Receiver Profile)
M0=S/ST;


function cnnnumgradcheck(net, x, y)
    epsilon = 1e-4;
    er      = 1e-8;
    n = numel(net.layers);
    for j = 1 : numel(net.ffb)
        net_m = net; net_p = net;
        net_p.ffb(j) = net_m.ffb(j) + epsilon;
        net_m.ffb(j) = net_m.ffb(j) - epsilon;
        net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
        net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
        d = (net_p.L - net_m.L) / (2 * epsilon);
        e = abs(d - net.dffb(j));
        if e > er
            error('numerical gradient checking failed');
        end
    end

    for i = 1 : size(net.ffW, 1)
        for u = 1 : size(net.ffW, 2)
            net_m = net; net_p = net;
            net_p.ffW(i, u) = net_m.ffW(i, u) + epsilon;
            net_m.ffW(i, u) = net_m.ffW(i, u) - epsilon;
            net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
            net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
            d = (net_p.L - net_m.L) / (2 * epsilon);
            e = abs(d - net.dffW(i, u));
            if e > er
                error('numerical gradient checking failed');
            end
        end
    end

    for l = n : -1 : 2
        if strcmp(net.layers{l}.type, 'c')
            for j = 1 : numel(net.layers{l}.a)
                net_m = net; net_p = net;
                net_p.layers{l}.b{j} = net_m.layers{l}.b{j} + epsilon;
                net_m.layers{l}.b{j} = net_m.layers{l}.b{j} - epsilon;
                net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
                net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
                d = (net_p.L - net_m.L) / (2 * epsilon);
                e = abs(d - net.layers{l}.db{j});
                if e > er
                    error('numerical gradient checking failed');
                end
                for i = 1 : numel(net.layers{l - 1}.a)
                    for u = 1 : size(net.layers{l}.k{i}{j}, 1)
                        for v = 1 : size(net.layers{l}.k{i}{j}, 2)
                            net_m = net; net_p = net;
                            net_p.layers{l}.k{i}{j}(u, v) = net_p.layers{l}.k{i}{j}(u, v) + epsilon;
                            net_m.layers{l}.k{i}{j}(u, v) = net_m.layers{l}.k{i}{j}(u, v) - epsilon;
                            net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
                            net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
                            d = (net_p.L - net_m.L) / (2 * epsilon);
                            e = abs(d - net.layers{l}.dk{i}{j}(u, v));
                            if e > er
                                error('numerical gradient checking failed');
                            end
                        end
                    end
                end
            end
        elseif strcmp(net.layers{l}.type, 's')
%            for j = 1 : numel(net.layers{l}.a)
%                net_m = net; net_p = net;
%                net_p.layers{l}.b{j} = net_m.layers{l}.b{j} + epsilon;
%                net_m.layers{l}.b{j} = net_m.layers{l}.b{j} - epsilon;
%                net_m = cnnff(net_m, x); net_m = cnnbp(net_m, y);
%                net_p = cnnff(net_p, x); net_p = cnnbp(net_p, y);
%                d = (net_p.L - net_m.L) / (2 * epsilon);
%                e = abs(d - net.layers{l}.db{j});
%                if e > er
%                    error('numerical gradient checking failed');
%                end
%            end
        end
    end
%    keyboard
end