%IKINE_SYM  Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT.  The cells of Q represent the
% solutions for each joint, ie. Q{1} is the solution for joint 1.  A
% cell may contain an array of solutions. The solution is expressed in terms
% of other joint angles and elements of the desired end-point pose which is
% represented by the symbolic matrix (3x4) with elements
%      nx ox ax tx
%      ny oy ay ty
%      nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N is the number of joint angles solved for.
%
% Options::
%
% 'file',F    Write the solution to an m-file named F
% 'Tpost',T   Add a symbolic 4x4 matrix T to the end of the chain
%
% Example::
%
%         mdl_planar2
%         sol = p2.ikine_sym(2);
%         length(sol)
%
%         q1 = sol{1}   % are the solution for joint 1
%         q2 = sol{2}   % is the solution for joint 2
%         length(q1)
%         ans =
%               2     % there are 2 solutions for this joint
%         q1(1)       % one solution for q1
%         q1(2);      % the other solution for q1
%
% Notes::
% - ignores tool and base transforms.
%
% References::
% - Robot manipulators: mathematics, programming and control
%   Richard Paul, MIT Press, 1981.
% - The kinematics of manipulators under computer control,
%   D.L. Pieper, Stanford report AI 72, October 1968.
%
% Notes::
% - Requires the MATLAB Symbolic Math Toolbox.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function out = ikine_sym(robot, N, varargin)

    %
    % Given a robot model the following steps are performed:
    % 1. Convert model to symbolic form
    % 2. Find relevant trig equations and solve them for joint angles
    % 3. Write an M-file to implement the solution
    %      xikine(T)
    %      xikine(T, S) where S is a 3 vector with elements 1 or 2 to select
    %       the first or second solution for the corresponding joint.
    %
    % TODO:
    %  - handle the wrist joints, only first 3 joints so far
    %  - handle base and tool transforms
    %  - allow 3DOF solution which is rotation, ie. wrist

    opt.file = [];
    opt.Tpost = [];
    opt.all = false;
    opt = tb_optparse(opt, varargin);

    % make a symbolic representation of the passed robot
    srobot = SerialLink(robot);  % make a deep copy
    srobot = sym(srobot);  % convert to symbolic
    q = srobot.gencoords();

    % define symbolic elements of the homogeneous transform
    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real
    syms d4 real

    % inits
    Q = {};
    trigsubOld = [];
    trigsubNew = [];

    % loop over each joint variable
    for j=1:N
        fprintf('----- solving for joint %d\n', j);

        % create some equations to sift through
        [left,right] = pieper(srobot, j, 'left', opt.Tpost);

        % decide which equations to look at
        if j <= 3
            % for first three joints only focus on translational part
            left = left(1:3, 4); left = left(:);
            right = right(1:3, 4); right = right(:);
        else
            % for last three joints only focus on rotational part
            left = left(1:3, 1:3); left = left(:);
            right = right(1:3, 1:3); right = right(:);
        end

        % substitute sin/cos for preceding joint as Sj/Cj, essentially removes
        % the joint variables from the equations and treats them as constants.
        if ~isempty(trigsubOld)
            left = rsubs(left, trigsubOld, trigsubNew);
            right = rsubs(right, trigsubOld, trigsubNew);
        end

        % then simplify the LHS
        %   do it after the substitution to prevent sum of angle terms being introduced
        left = simplify(left);

        % search for a solveable equation:
        %    function of current joint variable on the LHS
        %    constant element on the RHS
        k = NaN;
        for i=1:length(left)
            fprintf('%d: %s\n', i, char(left(i) == right(i)));
            if hasonly(left(i), j) && isconstant(right(i))
                k = i;
            end
        end

        eq = [];

        if ~isnan(k)
            % create the equation to solve: LHS-RHS == 0
            fprintf('choosing equation %d\n', k);

            eq = left(k) - right(k);
        else
            % ok, we weren't lucky, try another strategy

            % find all equations:
            %    function of current joint variable on the LHS

            k = [];
            for i=1:length(left)
                % has qj on the left
                if hasonly(left(i), j)
                    k = [k i];
                end
            end

            % hopefully we found at least two of them
            if length(k) < 2
                continue;
            end

            % we did, lets see if the sum square RHS is constant
            for kk = nchoosek(k, 2)'
                fprintf('No simple equation, let''s square and add RHS %d %d\n', kk);
                rhs = simplify(right(kk(1))^2 + right(kk(2))^2); % was simple
                if isconstant( rhs )
                    eq = simplify( expand( left(kk(1))^2 + left(kk(2))^2 ) ) - rhs;
                    break
                end
            end
            if isempty(eq)
                fprintf('** can''t solve this equation, out of options');
                k
                left(k)==right(k)
                error('can''t solve this equation');
            end
            % ensure that S^2+C^2=1 substitutions are made
            eq = subs(eq, trigsubOld, trigsubNew);
            disp(eq)
        end

        % expand the list of joint variable subsitutions
        fprintf('subs sin/cos q%d for S/C\n', j);
        trigsubOld = [trigsubOld mvar('sin(q%d)', j)^2+mvar('cos(q%d)', j)^2 mvar('sin(q%d)', j) mvar('cos(q%d)', j) ];
        trigsubNew = [trigsubNew 1 mvar('S%d', j) mvar('C%d', j) ];

        % now solve the equation
        if srobot.links(j).isrevolute()
            % for revolute joint it will be a trig equation, do we know how to solve it?
            Q{j} = solve_joint(eq, j );
            if isempty(Q)
                warning('RTB:ikine_sym', 'can''t solve this kind of equation');
            end
        else
            fprintf('prismatic case\n')
            q = mvar('q%d', j);
            Q{j} = solve( eq == 0, q);
        end
    end

    % final simplification
    %  get rid of C^2+S^2 and C^4, S^4 terms
    fprintf('**final simplification pass\n')

    trigsubOld = [];
    trigsubNew = [];
    for j=1:N
        trigsubOld = [trigsubOld mvar('S%d', j) mvar('C%d', j) ];
        trigsubNew = [trigsubNew mvar('sin(q%d)', j) mvar('cos(q%d)', j) ];
    end

    Q = simplify_powers(Q, N, trigsubOld, trigsubNew);

    % Q is a cell array of equations for joint variables
    if nargout > 0
        out = Q;
    end

    if ~isempty(opt.file)
        fprintf('**generate MATLAB code\n')
        gencode(Q);
    end
end

%PIEPER Return a set of equations using Pieper's method
%
% [L,R] = pieper(robot, n, which)
%
% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of
%
%  A1' T = A2 A3 A4     n=1, which='left'
%  A2' A1' T = A3 A4    n=2, which='left'
%  A3' A2' A1' T = A4   n=3, which='left'
%
%  T A4' = A1 A2 A3     n=1, which='right'
%  T A4' A3' = A1 A2    n=2, which='right'
%  T A4' A3' A2' = A1   n=3, which='right'
%
% A' denotes inversion not transposition
%
% Judicious choice of the equations can lead to joint solutions

function [L,R] = pieper(robot, n, which, Tpost)

    if nargin < 3
        which = 'left';
    end
    if nargin < 4 || isempty(Tpost)
        Tpost = transl(zeros(robot.n, 3));
    end
    assert(n <= robot.n, 'RTB:ikine_sym:badarg', 'N is greater than number of joints');

    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real

    T = [nx ox ax tx
        ny oy ay ty
        nx oz az tz
        0  0  0  1 ];

    T = inv(robot.base.T) * T * inv(robot.tool.T);

    q = robot.gencoords();

    % Create the symbolic A matrices
    for j=1:robot.n
        A{j} = robot.links(j).A(q(j)).T * Tpost(:,:,j);
    end

    switch which
        case 'left'
            left = T;
            for j=1:n
                left = inv(A{j}) * left ;
            end

            right = eye(4,4);
            for j=n+1:robot.n
                right = right * A{j};
            end

        case 'right'
            left = T;
            for j=1:n
                left = left * inv(A{robot.n-j+1});
            end

            right = eye(4,4);
            for j=1:(robot.n-n)
                right = right * A{j};
            end
    end

    %     left = simple(left);
    %     right = simple(right);

    if nargout == 0
        left == right
    elseif nargout == 1
        L = left;
    elseif nargout == 2
        L = left;
        R = right;
    end
end

%SOLVE_JOINT Solve a trigonometric equation
%
% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ.
% The result is a vector of symbolic solutions.
%
% The equations must be of the form:
%  A cos(qJ) + B sin(qJ) = 0
%  A cos(qJ) + B sin(qJ) = C
%
% where A, B, C are arbitrarily complex expressions.  qJ can be the only
% joint variable in the expression.
%
% Notes::
% - In general there are two solutions, but if A^2+B^2-C^2 = 0, then only one
%   solution is returned.
% - The one solution case may not be detected symbolically, in which case
%   the two returned solutions will have the same value after numerical
%   substitution
% - The symbolic solution may not be evaluteable numerically with
%   certain parameter values, ie. if A^2+B^2-C^2 < 0

function s = solve_joint(eq, j)

    % see http://petercorke.com/wordpress/solving-trigonometric-equations

    sinj = mvar('sin(q%d)', j);
    cosj = mvar('cos(q%d)', j);

    A = getcoef(eq, cosj);
    B = getcoef(eq, sinj);

    if isempty(A) || isempty(B)
        warning('don''t know how to solve this kind of equation');
    end

    C = -simplify(eq - A*cosj - B*sinj);

    A = simplify_sumsq(A, j);
    B = simplify_sumsq(B, j);
    C = simplify_sumsq(C, j);

    fprintf('A = %s\n', char(A));
    fprintf('B = %s\n', char(B));
    fprintf('C = %s\n', char(C));

    if C == 0
        fprintf('Solve for C == 0\n');
        % A cos(q) + B sin(q) = 0
        s(1) = atan2(A, -B);
        s(2) = atan2(-A, B);
    else
        fprintf('Solve for C != 0\n');
        % A cos(q) + B sin(q) = C
%         r = sqrt(A^2 + B^2 - C^2);
% %         phi = atan2(A, B);
% %
% %         s(2) = atan2(C, r) - phi;
% %         s(1) = atan2(C, -r) - phi;
%         if r == 0
%             s = atan2(B*C, A*C)
%         else
%             s(1) = atan2(B*C + A*r, A*C - B*r);
%             s(2) = atan2(B*C - A*r, A*C + B*r);
%         end
        d = sqrt( simplify(B^2-C^2+A^2) );
        fprintf('d = %s\n', char(d));

        s(1) = atan2(B*C + A*d, A*C - B*d);
        s(2) = atan2(B*C - A*d, A*C + B*d);
    end

%     simplify( A*cos(s(1)) + B*sin(s(1)) - C )
%     simplify( A*cos(s(2)) + B*sin(s(2)) - C )

    if nargout == 0
        try
            eval(s)
        catch
            s
        end
    end
end

function Qout = simplify_sumsq(Q, N)

    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2+mvar('C%d', j)^2];
        tsubNew = [tsubNew         1                        ];
    end
    Q = simplify( rsubs(Q, tsubOld, tsubNew) );
    Qout = simplify( rsubs(Q, tsubOld, tsubNew) );
end

function Qout = simplify_powers(Q, N, trigsubOld, trigsubNew)

        % create a list of simplifications
    %  substitute S^2 = 1-C^2, S^4=(1-C^2)^2
    fprintf('power simplification\n');
    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2+mvar('C%d', j)^2 mvar('S%d', j)^4];
        tsubNew = [tsubNew         1                        (1-mvar('C%d', j)^2)^2];
    end

    for j=1:length(Q)
        for k=1:5
            % seem to need to iterate this, not quite sure why
            Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) );
        end
        % subs Sx, Cx to sin(qx), cos(qx)
        Qout{j} = simplify( subs(Q{j}, trigsubOld, trigsubNew) );
    end
end

function out = rsubs(eq, old, new)
    for i = 1:length(old)
        eq = subs(eq, old(i), new(i));
    end
    out = eq;
end

%MVAR Create a symbolic variable
%
% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF
%
% eg. mvar('q%d', j)
%
% The symbolic is explicitly declared to be real.

function v = mvar(fmt, varargin)

    if isempty(strfind(fmt, '('))
        % not a function
        v = sym( sprintf(fmt, varargin{:}), 'real' );
    else
        v = str2sym( sprintf(fmt, varargin{:}) );

    end
end

%HASONLY Determine if an expression contains only certain joint variables
%
% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E
% are listed in the vector L.
%
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false

function s = hasonly(eq, j)

    q = findq(eq);
    if isempty(q)
        s = false;
    else
        s = all(ismember(j, findq(eq)));
    end
end

%ISCONSTANT Determine if an expression is free of joint variables
%
% S = ISCONSTANT(E) is true if the expression E contains no joint variables such
% q1, q2 etc.

function s = isconstant(eq)
    s = isempty(findq(eq));
end

%FINDQ Find the joint variables in expression
%
% Q = FINDQ(E) returns a list of integers indicating the joint variables found
% in the expression E.  For instance an instance of 'q1' would cause a 1 to be
% returned and so on.
%
% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2]

function q = findq(s)

    q = [];

    for var=symvar(s)
        if isempty(var)
            break
        end
        varname = char(var);
        if varname(1) == 'q'
            q = [q str2num(varname(2:end))];
        end
    end
end

function coef = getcoef(eq, trig)
    z = children( collect(eq, trig) );
    z = children( z(1) );
    coef = z(1);
end

% Output a joint expression to a file

function s = gencode(Q, filename)

    function s = G(s, fmt, varargin)
        s = strvcat(s, sprintf(fmt, varargin{:}));
    end

    s = 'function q = xikine(T, sol)';
    s = G(s, '  if nargin < 2; sol = ones(1, %d); end', length(Q));
    s = G(s, '  px = T(1,4); py = T(2,4); pz = T(3,4);');

    for j=1:3
        Qj = Q{j};   % cast it to subclass
        if length(Qj) == 1
            s = G(s, '  q(%d) = %s', j, matgen2(Qj));
        elseif length(Qj) == 2
            s = G(s, '  if sol(%d) == 1', j);
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(1)));
            s = G(s, '  else');
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(2)));
            s = G(s, '  end');

        end

        s = G(s, '  S%d = sin(q(%d));', j, j);
        s = G(s, '  C%d = cos(q(%d));', j, j);
        s = G(s, ' ');

    end
    s = G(s, 'end');

    fp = fopen(filename, 'w');
    for i=1:numrows(s)
        fprintf(fp, '%s\n', deblank(s(i,:)));
    end
    fclose(fp);

end

% Generate MATLAB code from an expression
%
% Requires a bit of a hack, a subclass of sym (sym2) to do this

function s = matgen2(e)

    s = matgen(sym2(e));

    k = strfind(s, '=');
    s = deblank( s(k+2:end) );
end


% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% DO VARIOUS ROBOTS: 2 LINK, HYPER2D, HYPER3D

% Inverse kinematics is the problem of finding the robot joint coordinates,
% given a homogeneous transform representing the last link of the manipulator.
% It is very useful when the path is planned in Cartesian space, for instance
% a straight line path as shown in the trajectory demonstration.
%
% First generate the transform corresponding to a particular joint coordinate,

mdl_puma560
q = [0 -pi/4 -pi/4 0 pi/8 0]
T = p560.fkine(q)

% Now the inverse kinematic procedure for any specific robot can be derived
% symbolically and in general an efficient closed-form solution can be
% obtained.  However we are given only a generalized description of the
% manipulator in terms of kinematic parameters so an iterative solution will
% be used. The procedure is slow, and the choice of starting value affects
% search time and the solution found, since in general a manipulator may
% have several poses which result in the same transform for the last
% link. The starting point for the first point may be specified, or else it
% defaults to zero (which is not a particularly good choice in this case)

qi = p560.ikine(T);

% and in fact it does not converge

qi

% We can help the solution along by using the 'pinv' option

qi = p560.ikine(T, 'pinv');
% and the result
qi
% is the same as the original set of joint angles
q

% However in general this will not be the case, there are multiple
% solutions, and the solution that is found depends on the initial
% choice of angles.
%
% A more efficient approach is to use an analytic solution and the toolbox
% supports the common case of a 6-axis robot arm with a spherical wrist

qi = p560.ikine6s(T)
% which is different to the original joint angles, but as expected

p560.fkine(qi)

% it does give the same end-effector pose.

% The analytic solution allows the specific solution to be specified
% using a character string and to get the same set of joint angles

p560.ikine6s(T, 'rdf')
% where we have specified that the robot is in a right-handed configuration
% (r), with its elbow down (d), and the wrist flipped (f).

% A solution is not always possible, for instance if the specified
% transform describes a point out of reach of the manipulator.  As
% mentioned above the solutions are not necessarily unique, and there
% are singularities at which the manipulator loses degrees of freedom
% and joint coordinates become linearly dependent.

% Inverse kinematics may also be computed for a trajectory.
% If we take a Cartesian straight line path between two poses in 50 steps

T1 = transl(0.6, -0.5, 0.0) % define the start point
T2 = transl(0.4, 0.5, 0.2)	% and destination
T = ctraj(T1, T2, 50); 	% compute a Cartesian path

% now solve the inverse kinematics

q = p560.ikine6s(T);
about q
% which has one row per time step and one column per joint angle

% Let's examine the joint space trajectory that results in straightline
% Cartesian motion

subplot(3,1,1); plot(q(:,1)); xlabel('Time (s)'); ylabel('Joint 1 (rad)');
subplot(3,1,2); plot(q(:,2)); xlabel('Time (s)'); ylabel('Joint 2 (rad)');
subplot(3,1,3); plot(q(:,3)); xlabel('Time (s)'); ylabel('Joint 3 (rad)');

% This joint space trajectory can now be animated
clf
p560.plot(q)


function [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,options)
% spike inference using a constrained deconvolution approach:
%      min      sum(sp)
%    c,sp,b,c1
%      subject to: sp >= 0
%                   b >= 0
%                  G*c = sp
%                   c1 >= 0
%           ||y-b-c - c_in|| <= sn*sqrt(T)

%   Variables:
%   y:      raw fluorescence data (vector of length(T))
%   c:      denoised calcium concentration (Tx1 vector)
%   b:      baseline concentration (scalar)
%  c1:      initial concentration (scalar)
%   g:      discrete time constant(s) (scalar or 2x1 vector)
%  sn:      noise standard deviation (scalar)
%  sp:      spike vector (Tx1 vector)

%   USAGE:
%   [c,b,c1,g,sn,sp] = constrained_foopsi(y,b,c1,g,sn,OPTIONS)
%   The parameters b,cin,g,sn can be given or else are estimated from the data

%   OPTIONS: (stuct for specifying options)
%         p: order for AR model, used when g is not given (default 2)
%    method: methods for performing spike inference
%   available methods: 'dual' uses dual ascent
%                       'cvx' uses the cvx package available from cvxr.com (default)
%                      'lars' uses the least regression algorithm
%                     'spgl1' uses the spgl1 package available from
%                     math.ucdavis.edu/~mpf/spgl1/  (usually fastest)
%   bas_nonneg:   flag for setting the baseline lower bound. if 1, then b >= 0 else b >= min(y)
%   noise_range:  frequency range over which the noise power is estimated. Default [Fs/4,Fs/2]
%   noise_method: method to average the PSD in order to obtain a robust noise level estimate
%   lags:         number of extra autocovariance lags to be considered when estimating the time constants
%   resparse:     number of times that the solution is resparsened (default 0). Currently available only with methods 'cvx', 'spgl'
%   fudge_factor: scaling constant to reduce bias in the time constant estimation (default 1 - no scaling)

% Written by:
% Eftychios A. Pnevmatikakis, Simons Foundation, 2015

defoptions.p = 2;
defoptions.method = 'cvx';
defoptions.bas_nonneg = 1;              % nonnegativity option for baseline estimation
defoptions.noise_range = [0.25,0.5];    % frequency range over which to estimate the noise
defoptions.noise_method = 'logmexp';    % method for which to estimate the noise level
defoptions.lags = 5;                    % number of extra lags when computing the AR coefficients
defoptions.resparse = 0;                % number of times to re-sparse solution
defoptions.fudge_factor = 1;            % fudge factor for time constants

if nargin < 6
    options = defoptions;
    if nargin < 5
        sn = [];
        if nargin < 4
            g = [];
            if nargin < 3
                c1 = [];
                if nargin < 2
                    b = [];
                end
            end
        end
    end
end

if ~isfield(options,'p');  options.p = defoptions.p;  end
if ~isfield(options,'method'); options.method = defoptions.method; end
if ~isfield(options,'bas_nonneg'); options.bas_nonneg = defoptions.bas_nonneg; end
if ~isfield(options,'noise_range'); options.noise_range = defoptions.noise_range; end
if ~isfield(options,'noise_method'); options.noise_method = defoptions.noise_method; end
if ~isfield(options,'lags'); options.lags = defoptions.lags; end
if ~isfield(options,'resparse'); options.resparse = defoptions.resparse; end
if ~isfield(options,'fudge_factor'); options.fudge_factor = defoptions.fudge_factor; end

method = options.method;
if isempty(b);
    bas_est = 1;
else
    bas_est = 0;
end
if isempty(c1)
    c1_est = 1;
else
    c1_est = 0;
end

y = y(:);
T = length(y);
y_full = y;
mis_data = isnan(y);
E = speye(T);
E(mis_data,:) = [];

if any(mis_data)
    y_full(mis_data) = interp1(find(~mis_data),y(~mis_data),find(mis_data));
end

if isempty(sn)
    sn = GetSn(y_full,options.noise_range,options.noise_method);
end
if isempty(g)
    g = estimate_time_constants(y_full,options.p,sn,options.lags);
    while max(abs(roots([1,-g(:)']))>1) && options.p < 5
        warning('No stable AR(%i) model found. Checking for AR(%i) model \n',options.p,options.p+1);
        options.p = options.p + 1;
        g = estimate_time_constants(y,options.p,sn,options.lags);
    end
    if options.p == 5
        g = 0;
    end
    %fprintf('Stable AR(%i) model found \n',options.p);
    % re-adjust time constant values
    rg = roots([1;-g(:)]);
    if ~isreal(rg); rg = real(rg) + .001*randn(size(rg)); end
    rg(rg>1) = 0.95 + 0.001*randn(size(rg(rg>1)));
    rg(rg<0) = 0.15 + 0.001*randn(size(rg(rg<0)));
    pg = poly(options.fudge_factor*rg);
    g = -pg(2:end);
end
if options.bas_nonneg  % lower bound for baseline
    b_lb = 0;
else
    b_lb = min(y);
end

if strcmpi(method,'dual'); method = 'dual';
elseif strcmpi(method,'cvx'); method = 'cvx';
elseif strcmpi(method,'lars'); method = 'lars';
elseif strcmpi(method,'spgl1'); method = 'spgl1';
else fprintf('Invalid choice of method. Using CVX \n'); method = 'cvx';
end

if strcmpi(method,'dual') && any(mis_data)
    warning('Dual method does not support missing data. Switching to CVX');
    method = 'cvx';
end

if options.resparse > 0 && (strcmpi(method,'dual') || strcmpi(method,'lars'))
    warning('Resparsening is not supported with chosen method. Switching to CVX');
    method = 'cvx';
end

pathCell = regexp(path, pathsep, 'split');
g = g(:);
G = spdiags(ones(T,1)*[-g(end:-1:1)',1],-length(g):0,T,T);
gd = max(roots([1,-g']));  % decay time constant for initial concentration
gd_vec = gd.^((0:T-1)');

switch method
    case 'dual'
         v = G'*ones(T,1);
        thr = sn*sqrt(T-sum(mis_data));
        if bas_est; b = 0; end
        if c1_est; c1 = 0; end
        myfun = @(Ald) lagrangian_temporal_gradient(Ald,thr^2,y(~mis_data)-b-c1*gd_vec(~mis_data),bas_est,c1_est);
        c = [G\max(G*y,0);zeros(bas_est);zeros(c1_est)];
        options_dual = optimset('GradObj','On','Display','Off','Algorithm','interior-point','TolX',1e-8);
        ld_in = 10;
        [ld,~,flag] = fmincon(myfun,ld_in,[],[],[],[],0,[],[],options_dual);
        if (flag == -2) || (flag == -3)
            warning('Problem seems unbounded or infeasible. Try a different method.');
        end
        if bas_est; b = c(T+bas_est); end
        if c1_est; c1 = c(end); end
        c = c(1:T);
        sp = G*c;
    case 'cvx'
        onPath = ~isempty(which('cvx_begin'));
        if onPath
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                [c(:,rep),bas(rep),cin(rep)] = cvx_foopsi(y,b,c1,sn,b_lb,g,w_,~mis_data);
                sp(:,rep) = G*c(:,rep);
                w_ = 1./(max(sp(:,rep),0) + 1e-8);
            end
            sp(sp<1e-6) = 0;
            c = G\sp;
            b = bas;
            c1 = cin;
        else
            error('CVX does not appear to be on the MATLAB path. It can be downloaded from cvxr.com \n');
        end
    case 'lars'
         Ginv = E*[full(G\speye(T)),ones(T,bas_est),gd_vec*ones(1,c1_est)];
         if bas_est; b = 0; end
         if c1_est; c1 = 0; end
         [~, ~, spikes, ~, ~] = lars_regression_noise(y(~mis_data)-b_lb*bas_est - b - c1*gd_vec(~mis_data), Ginv, 1, sn^2*(T-sum(mis_data)));
         sp = spikes(1:T);
         b = (spikes(T+bas_est)+b_lb)*bas_est + b*(1-bas_est);
         c1 = spikes(end)*c1_est + c1*(1-c1_est);
         c = G\sp;
    case 'spgl1'
        onPath = ~isempty(which('spgl1'));
        if onPath
            Gx = @(x,mode) G_inv_mat(x,mode,T,g,gd_vec,bas_est,c1_est,E);
            c = zeros(T,1+options.resparse);
            sp = zeros(T,1+options.resparse);
            bas = zeros(1+options.resparse,1);
            cin = zeros(1+options.resparse,1);
            w_ = ones(T,1);
            for rep = 1:options.resparse+1
                if bas_est; b = 0; w_ = [w_;1e-10]; end
                if c1_est; c1 = 0; w_ = [w_;1e-10]; end
                options_spgl = spgSetParms('project',@NormL1NN_project ,'primal_norm', @NormL1NN_primal,'dual_norm',@NormL1NN_dual,'verbosity',0,'weights',w_);
                [spikes,r,~,~] = spg_bpdn( Gx, y(~mis_data)-b_lb*bas_est - (1-bas_est)*b-(1-c1_est)*c1*gd_vec(~mis_data), sn*sqrt(T-sum(mis_data)),options_spgl);
                c(:,rep) = G\spikes(1:T); %Gx([spikes(1:T);0],1);                                  %% calcium signal
                bas(rep) = b*(1-bas_est) + bas_est*spikes(T+bas_est)+b_lb*bas_est;       %% baseline
                cin(rep) = c1*(1-c1_est) + c1_est*spikes(end);
                sp(:,rep) = spikes(1:T);                                           %% spiking signal
                w_ = 1./(spikes(1:T)+1e-8);
            end
            b = bas;
            c1 = cin;
            %sn = norm(r)/sqrt(T);
        else
            error('SPGL1 does not appear to be on the MATLAB path. It can be downloaded from math.ucdavis.edu/~mpf/spgl1 \n');
        end
end

    function sn = GetSn(Y,range_ff,method)
        % estimate noise level with a power spectral density method
        L=length(Y);
%         if ~isempty(which('pmtm'))
%             [psd_Y,ff] = pmtm(Y,5/2,1000,1);
%         end
        if ~isempty(which('pwelch'));
            [psd_Y,ff]=pwelch(Y,round(L/8),[],1000,1);
        else
            xdft = fft(Y);
            xdft = xdft(:,1:round(L/2)+1);
            psd_Y = (1/L) * abs(xdft).^2;
            ff = 0:1/L:1/2;
            psd_Y(2:end-1) = 2*psd_Y(2:end-1);
        end
        ind=ff>range_ff(1);
        ind(ff>range_ff(2))=0;
        switch method
            case 'mean'
                sn=sqrt(mean(psd_Y(ind)/2));
            case 'median'
                sn=sqrt(median(psd_Y(ind)/2));
            case 'logmexp'
                sn = sqrt(exp(mean(log(psd_Y(ind)/2))));
        end
    end

    function g = estimate_time_constants(y,p,sn,lags)
        % estimate time constants from the sample autocovariance function

        lags = lags + p;
        if ~isempty(which('xcov')) %signal processing toolbox
            xc = xcov(y,lags,'biased');
        else
            ynormed = (y - mean(y));
            xc = nan(lags + 1, 1);
            for k = 0:lags
                xc(k + 1) = ynormed(1 + k:end)' * ynormed(1:end - k);
            end
            xc = [flipud(xc(2:end)); xc] / numel(y);
        end
        xc = xc(:);
        A = toeplitz(xc(lags+(1:lags)),xc(lags+(1:p))) - sn^2*eye(lags,p);
        g = pinv(A)*xc(lags+2:end);
    end

    function [f,grad] = lagrangian_temporal_gradient(Al,thr,y_raw,bas_flag,c1_flag)
        options_qp = optimset('Display','Off','Algorithm','interior-point-convex');
        H = [speye(T),ones(T,bas_flag),gd_vec*ones(1,c1_flag);...
            ones(bas_flag,T),T*ones(bas_flag),(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag);...
            (gd_vec*ones(1,c1_flag))',(1-gd^T)/(1-gd)*ones(c1_flag,bas_flag),(1-gd^(2*T))/(1-gd^2)*ones(c1_flag,c1_flag)];
        Ay = [y_raw;sum(y_raw)*ones(bas_flag);gd_vec'*y_raw*ones(c1_flag)];
        c = quadprog(2*Al(1)*H,[v;zeros(bas_flag+c1_flag,1)]-2*Al(1)*Ay,[-G,sparse(T,bas_flag+c1_flag);sparse(bas_flag+c1_flag,T),-speye(bas_flag+c1_flag)]...
            ,[sparse(T,1);-b_lb*ones(bas_flag);zeros(c1_flag)],[],[],[],[],c,options_qp);
        f = v'*c(1:T);
        grad = [sum((c(1:T)-y_raw + c(T+bas_flag)*bas_flag + c(end)*gd_vec*c1_flag).^2)-thr];
        f = f + Al(:)'*grad;
    end

    function b = G_inv_mat(x,mode,NT,gs,gd_vec,bas_flag,c1_flag,Emat)
        if mode == 1
            b = filter(1,[1;-gs(:)],x(1:NT)) + bas_flag*x(NT+bas_flag) + c1_flag*gd_vec*x(end);
            b = Emat*b;
           %b = G\x(1:NT) + x(NT+bas_flag)*bas_flag + x(end)*c1_flag;
        elseif mode == 2
            x = Emat'*x;
            b = [flipud(filter(1,[1;-gs(:)],flipud(x)));ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)];
           %b = [G'\x;ones(bas_flag,1)*sum(x);ones(c1_flag,1)*(gd_vec'*x)] ;
        end
    end

end


%% Low Level Parent Phase Reconstruction
%
%%
% In this section we discuss parent grain reconstruction at the example of
% a titanium alloy. Lets start by importing a sample data set

mtexdata alphaBetaTitanium

% the phase names for the alpha and beta phases
alphaName = 'Ti (alpha)';
betaName = 'Ti (Beta)';

% and plot the alpha phase as an inverse pole figure map
plot(ebsd(alphaName),ebsd(alphaName).orientations,'figSize','large')

%%
% The data set contains 99.8 percent alpha titanium and 0.2 percent beta
% titanium. Our goal is to reconstuct the original beta phase. The
% original grain structure appears almost visible for human eyes.
% Our computations will be based on the Burgers orientation relationship

beta2alpha = orientation.Burgers(ebsd(betaName).CS,ebsd(alphaName).CS)

%%
% that alligns (110) plane of the beta phase with the (0001) plane of the
% alpha phase and the [1-11] direction of the beta phase with the [2110]
% direction of the alpha phase.
%
% Note that all MTEX functions for parent grain reconstruction expect the
% orientation relationship as parent to child and not as child to parent.
%
%% Detecting triple points that belong to the same parent orientation
%
% In a first step we want to identify triple junctions that have
% misorientations that are compatible with a common parent orientations. To
% this end we first compute alpha grains using the option
% <QuadruplePoints.html |removeQuadruplePoints|> which turn all quadruple
% junctions into 2 triple junctions. Furthermore, we choose a very small
% threshold of 1.5 degree for the identification of grain boundaries to
% avoid alpha orientations that belong to different beta grains get merged
% into the same alpha grain.

% reconstruct grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'),'threshold',1.5*degree,...
  'removeQuadruplePoints');
grains = smooth(grains,1,'moveTriplePoints');

% plot all alpha pixels
region = [299 401 -500 -440];
plot(ebsd(alphaName),ebsd(alphaName).orientations,...
  'region',region,'micronbar','off','figSize','large');

% and on top the grain boundaries
hold on
plot(grains.boundary,'linewidth',2 ,'region',region);
hold off

%%
% Above we have plotted only a very small subregion of the original data
% set to make the seperation of the qudruple junctions better visible.
%
% Next we extract all alpha - alpha - alpha triple junctions and use the
% command <calcParent.html |calcParent|> to find for each of these triple
% junctions the best fitting parent orientations.

% extract all alpha - alpha - alpha triple points
tP = grains.triplePoints(alphaName,alphaName,alphaName)

% compute for each triple point the best fitting parentId and how well the fit is
tPori = grains(tP.grainId).meanOrientation;
[parentId, fit] = calcParent(tPori,beta2alpha,'numFit',2,'id','threshold',5*degree);

%%
% The command |calcParent| returns for each child orientation a |parentId|
% which allows us later to compute the parent orientation from the child
% orientation. Furthermore, the command return for each triple junction the
% misfit between the adjecent parent orientations in radiant. Finally, the
% option |'numFit',2| causes |calcParent| to return not only the best fit
% but also the second best fit. This will be used later. First we simple
% colorize the triple junctions according to the best fit.

hold on
plot(tP,fit(:,1) ./ degree,'MarkerEdgecolor','k','MarkerSize',10,'region',region)
setColorRange([0,5])
mtexColorMap LaboTeX
mtexColorbar
hold off

%%
% Next we select those triple junctions as reliable that have a fit less
% than 2.5 degree and second best fit that is larger than  2.5 degree

consistenTP = fit(:,1) < 2.5*degree & fit(:,2) > 2.5*degree;

% marke these triple points by a red cicle
hold on
plot(tP(consistenTP),'MarkerEdgecolor','r','MarkerSize',10,...
  'MarkerFaceColor','none','linewidth',2,'region',region)
hold off

%% Recover beta grains from consistent triple junctions
%
% We observe that despite the quite sharp threshold we have many consistent
% triple points. In the next step we check wether all consistent triple
% junctions of a grain vote for the same parent orientation. Such a check
% for consistent votes can be computed by the command <majorityVote.html
% |majorityVote|> using the option |strict|.

% get a unique parentId vote for each grain
[parentId, numVotes] = majorityVote( tP(consistenTP).grainId, ...
  parentId(consistenTP,:,1), max(grains.id),'strict');

%%
% The command |majorityVote| returns for each grain with consistent
% parentId votes this unique parentId and for all other grains |NaN|.
% The second  output argument gives the number of these votes
%
% For all grains with at least 3 unique vote we now use the command
% <variants.html |variants|> to compute the parent orientation
% corresponding to the |parentId|. This parent orientations we assign as
% new |meanOrientation| to our grains.

% lets store the parent grains into a new variable
parentGrains = grains;

% change orientations of consistent grains from child to parent
parentGrains(numVotes>2).meanOrientation = ...
  variants(beta2alpha,grains(numVotes>2).meanOrientation,parentId(numVotes>2));

% update all grain properties that are related to the mean orientation
parentGrains = parentGrains.update;

%%
% Lets plot map of these reconstructed beta grains

% define a color key
ipfKey = ipfColorKey(ebsd(betaName));
ipfKey.inversePoleFigureDirection = vector3d.Y;

% and plot
plot(parentGrains(betaName), ...
  ipfKey.orientation2color(parentGrains(betaName).meanOrientation),'figSize','large')

%%
% We observe that this first step already results in very many Beta grains.
% However, the grain boundaries are still the boundaries of the original
% alpha grains. To overcome this, we merge all Beta grains that have a
% misorientation angle smaller then 2.5 degree.
%
% As an additional consistency check we verify that each parent
% grain has been reconstructed from at least 2 child grains. To this end we
% first make a testrun the merge operation and then revert all parent
% grains that that have less then two childs. This step may not nessesary
% in many case.

% test run of the merge operation
[~,parentId] = merge(parentGrains,'threshold',2.5*degree,'testRun');

% count the number of neighbouring child that would get merged with each child
counts = accumarray(parentId,1);

% revert all beta grains back to alpha grains if they would get merged with
% less then 1 other child grains
setBack = counts(parentId) < 2 & grains.phaseId == grains.name2id(alphaName);
parentGrains(setBack).meanOrientation = grains(setBack).meanOrientation;
parentGrains = parentGrains.update;

%%
% Now we perform the actual merge and the reconstruction of the parent
% grain boundaries.

% merge beta grains
[parentGrains,parentId] = merge(parentGrains,'threshold',2.5*degree);

% set up a EBSD map for the parent phase
parentEBSD = ebsd;

% and store there the grainIds of the parent grains
parentEBSD('indexed').grainId = parentId(ebsd('indexed').grainId);

plot(parentGrains(betaName), ...
  ipfKey.orientation2color(parentGrains(betaName).meanOrientation),'figSize','large')

%% Merge alpha grains to beta grains
%
% After the first two steps we have quite some alpha grains have not yet
% transformed into beta grains. In order to merge those left over alpha
% grains we check whether their misorientation with one of the neighbouring
% beta grains coincides with the parent to grain orientation relationship
% and if yes merge them evantually with the already reconstructed beta
% grains.
%
% First extract a list of all neighbouring alpha - beta grains

% all neighbouring alpha - beta grains
grainPairs = neighbors(parentGrains(alphaName), parentGrains(betaName));

%%
% and check how well they fit to a common parent orientation

% extract the corresponding meanorientations
oriAlpha = parentGrains( grainPairs(:,1) ).meanOrientation;
oriBeta = parentGrains( grainPairs(:,2) ).meanOrientation;

% compute for each alpha / beta pair of grains the best fitting parentId
[parentId, fit] = calcParent(oriAlpha,oriBeta,beta2alpha,'numFit',2,'id');

%%
% Similarly, as in the first step the command <calcParent.html
% |calcParent|> returns a list of |parentId| that allows the convert the
% child orientations into parent orientations using the command
% <variants.html |variants|> and the fitting to the given parent
% orientation. Similarly, as for the triple point we select only those
% alpha beta pairs such that the fit is below the threshold of 2.5 degree
% and at the same time the second best fit is above 2.5 degree.

% consistent pairs are those with a very small misfit
consistenPairs = fit(:,1) < 5*degree & fit(:,2) > 5*degree;

%%
% Next we compute for all alpha grains the majority vote of the surounding
% beta grains and change their orientation from alpha to beta

parentId = majorityVote( grainPairs(consistenPairs,1), ...
  parentId(consistenPairs,1), max(parentGrains.id));

% change grains from child to parent
hasVote = ~isnan(parentId);
parentGrains(hasVote).meanOrientation = ...
  variants(beta2alpha, parentGrains(hasVote).meanOrientation, parentId(hasVote));

% update grain boundaries
parentGrains = parentGrains.update;

% merge new beta grains into the old beta grains
[parentGrains,parentId] = merge(parentGrains,'threshold',5*degree);

% update grainId in the ebsd map
parentEBSD('indexed').grainId = parentId(parentEBSD('indexed').grainId);

% plot the result
color = ipfKey.orientation2color(parentGrains(betaName).meanOrientation);
plot(parentGrains(betaName),color,'linewidth',2)

%%
% The above step has merged

sum(hasVote)

%%
% alpha grains into the already reconstructed beta grain. This reduces the
% amount of grains not yet reconstructed to

sum(parentGrains('Ti (alpha').grainSize) ./ sum(parentGrains.grainSize)*100

%%
% percent. One way to proceed would be to repeat the steps of this section
% multiple time, maybe with increasing threshold, until the percentage of
% reconstructed beta grains is sufficiently high. Another approach in to
% consider the left over alpha grains as noise and use denoising techniques
% to replace them with beta orientations. This will be done in the last
% section.

%% Reconstruct beta orientations in EBSD map
%
% Until now we have only recovered the beta orientations as the mean
% orientations of the beta grains. In this section we want to set up the
% EBSD variable |parentEBSD| to contain for each pixel a reconstruction of
% the parent phase orientation.
%
% Therefore, we first identify all pixels that previously have been alpha
% titanium but now belong to a beta grain.

% consider only original alpha pixels that now belong to beta grains

isNowBeta = parentGrains.phaseId(max(1,parentEBSD.grainId)) == ebsd.name2id(betaName) &...
  parentEBSD.phaseId == ebsd.name2id(alphaName);

%%
% Next we can use once again the function <calcParent.html |calcParent|> to
% recover the original beta orientation from the measured alpha orientation
% giving the mean beta orientation of the grain.

% update beta orientation
[parentEBSD(isNowBeta).orientations, fit] = calcParent(parentEBSD(isNowBeta).orientations,...
  parentGrains(parentEBSD(isNowBeta).grainId).meanOrientation,beta2alpha);

%%
% We obtain even a measure |fit| for the corespondence between the beta
% orientation reconstructed for a single pixel and the beta orientation of
% the grain. Lets visualize this measure of fit

% the beta phase
plot(parentEBSD(isNowBeta),fit ./ degree,'figSize','large')
mtexColorbar
setColorRange([0,5])
mtexColorMap('LaboTeX')

hold on
plot(parentGrains.boundary,'lineWidth',2)
hold off

%%
% Lets finaly plot the reconstructed beta phase

plot(parentEBSD(betaName),ipfKey.orientation2color(parentEBSD(betaName).orientations),'figSize','large')

%% Denoising of the reconstructed beta phase
% As promised we end our discussion by applying denoising techniques to
% fill the remaining holes of alpha grains. To this end we first
% reconstruct grains from the parent orientations and throw away all small
% grains

[parentGrains,parentEBSD.grainId] = calcGrains(parentEBSD('indexed'),'angle',5*degree);

% remove all the small grains
parentEBSD = parentEBSD(parentGrains(parentGrains.grainSize > 15));

% redo grain reconstruction
[parentGrains,parentEBSD.grainId] = calcGrains(parentEBSD('indexed'),'angle',5*degree);

% smooth the grains a bit
parentGrains = smooth(parentGrains,5);

%%
% Finally, we denoise the remaining beta orientations and at the same time
% fill the empty holes. We choose a very small smoothing parameter |alpha|
% to keep as many details as possible.

F= halfQuadraticFilter;
F.alpha = 0.1;
parentEBSD = smooth(parentEBSD,F,'fill',parentGrains);

% plot the resulting beta phase
plot(parentEBSD(betaName),ipfKey.orientation2color(parentEBSD(betaName).orientations),'figSize','large')

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off

%%
% For comparison the map with original alpha phase and on top the recovered
% beta grain boundaries

plot(ebsd(alphaName),ebsd(alphaName).orientations,'figSize','large')

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off

%% Summary of relevant thresholds
%
% In parent grain reconstruction several parameters are involve are
% decicive for the success of the reconstruction
%
% * threshold for initial grain segmentation (1.5*degree)
% * maximum misfit at triple junctions (2.5 degree)
% * minimal misfit of the second best solution at triple junctions (2.5 degree)
% * minimum number of consistent votes (2)
% * threshold for merging beta grains (can be skipped)
% * threshold for merging alpha and beta grains (2.5 degree)
%%
%
% Visualize the misorientation to the mean reveals quite some fine
% structure in the reconstructed parent orientations.

cKey = axisAngleColorKey;
color = cKey.orientation2color(parentEBSD(betaName).orientations, parentGrains(parentEBSD(betaName).grainId).meanOrientation);
plot(parentEBSD(betaName),color)

hold on
plot(parentGrains.boundary,'lineWidth',3)
hold off


function Gamma = initGamma_random(T,K,D,Pstructure,Pistructure,episodic,priorOFFvsON)
% Return a Gamma timecourse corresponding to random transitions with a given
% dirichletdiag (i.e. the transition probability prior)
% For ehmm , D must be a 2x2 matrix of prior transitions - or left
%   empty
%   first state is ON, second is OFF  (always start in OFF)
%
% Author: Romesh Abeysuriya, University of Oxford (2017)
%         Diego Vidaurre, Aarhus University (2020)

if nargin < 4, Pstructure = []; end
if nargin < 5, Pistructure = []; end
if nargin < 6, episodic = false; end
if nargin < 7, priorOFFvsON = 3; end

rng('default')
rng('shuffle') % make this "truly" random

% Form transition probability matrix
if episodic
    P = (D-1)*eye(2)+ones(2);
    P(:,2) = P(:,2) * priorOFFvsON;
    P = bsxfun(@rdivide,P,sum(P,2));
    Pi = [0 1];
    if ~isempty(Pstructure) || ~isempty(Pistructure)
       warning('initGamma_random: Pstructure and Postructure will be ignored')
    end
else
    P = (D-1)*eye(K)+ones(K);
    if nargin>=4, P(~Pstructure) = 0; end
    if nargin<5, Pistructure = true(1,K); end
    P = bsxfun(@rdivide,P,sum(P,2));
    Pi = zeros(1,K);
    Pi(Pistructure) = 1;
    Pi = Pi / sum(Pi);
end

% Preallocate
Gamma = zeros(sum(T),K);

if episodic
    for n = 1:length(T)
        for k = 1:K
            while true
                gamma = zeros(T(n),2);
                gamma(1,2) = 1;
                for t = 2:T(n)
                    gamma(t,:) = mnrnd(1,P(gamma(t-1,:)==1,:));
                end
                if all(sum(gamma)>0), break; end
            end
            gamma = gamma + 0.0001 * rand(T(n),2);
            gamma = gamma ./ repmat(sum(gamma,2),1,2);
            t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
            Gamma(t0+1:t1,k) = gamma(:,1);
        end
    end
elseif isChain(P) % If it is a chain, it P must be ordered
    for n = 1:length(T)
        ch = cumsum(rand(1,K));
        ch = ch / ch(end);
        ch = round(T(n) * ch);
        ch = [0 ch(1:end-1)];
        gamma = zeros(T(n),K);
        for k = 1:K-1, gamma(ch(k)+1:ch(k+1),k) = 1; end
        gamma(ch(k+1)+1:end,K) = 1;
        t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
        Gamma(t0+1:t1,:) = gamma;
    end
else
    for n = 1:length(T)
        gamma = zeros(T(n),K);
        if sum(Pi>0)==1, gamma(1,Pi>0) = 1;
        else, gamma(1,:) = mnrnd(1,Pi);
        end
        for t = 2:T(n)
            gamma(t,:) = mnrnd(1,P(gamma(t-1,:)==1,:));
        end
        gamma = gamma + 0.0001 * rand(T(n),K);
        t0 = sum(T(1:n-1)); t1 = sum(T(1:n));
        Gamma(t0+1:t1,:) = gamma ./ repmat(sum(gamma,2),1,K);
    end
end

end

function bool = isChain(P)
%bool = any(sum(P,2)==1);
P = P~=0;
bool = all(sum(P(1:end-1,:),2)==2) && sum(P(end,:))==1;
end


% LCM - Variational Bayesian inference for latent component models.

% Likelihood: p(Y|A,S,C) = N(Y|A*S,C)
%
% Priors p(A) and p(S) can be arbitrary, because the user must specify
% the posterior estimation function.
%
% Covariance could be a Kronecker product of two arbitrary (or diagonal?)
% covariance matrices C_a and C_s? Missing values cause a nasty problem
% to Kronecker-covariance.. :(
%
% What if also an arbitrary diagonal covariance matrix C_d? The full
% joint covariance matrix would be:
%
% C = sqrt(C_d) * kron(C_a,C_s) * sqrt(C_d)
%
% Then missing values could be considered by using large (infinite)
% values in C_d..?

% Last modified 2010-10-12
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function Q = lcm(Y, D, varargin)

[M,N] = size(Y);

options = struct( ...
    'init',         [],               ...
    'prior',        [],               ...
    'common_tau',   true,             ...
    'common_nu',    false,            ...
    'rotate',       1,                ...
    'update_alpha', 1,               ...
    'update_beta',  1,               ...
    'update_nu',    1,               ...
    'autosavetime', nan,              ...
    'autosavefile', 'vbrfa_autosave', ...
    'maxiter',      100,              ...
    'robustness',   'independent-t', ... % none / multivariate-t / independent-t
    'user_data',    []);

% Parse arguments
[options, errmsg] = argparse( options, varargin{:} );
error(errmsg);

% Missing values
Obs = ~isnan(Y);
[ObsM, ObsN] = find(Obs);

% Helpful variables
Id = speye(D);
log2pi = log(2*pi);

logP = -inf;
loglikelihood = nan*zeros(options.maxiter,1);
start_time = cputime;
time = nan*zeros(options.maxiter,1);

lastsave = now;

for iter=1:options.maxiter

  t = cputime;

  %%%%%%%%%%%%%%%%%%%%%%
  %% Update parameters

  %%%%%%%%%%%%%%%%%
  %
  % X BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %% UPDATE X

  % q(X)
  [X, XX] =
  for n=1:N
    Mmv = Obs(:,n);
% $$$     UTau = U(Mmv,n).*tau(Mmv);
% $$$     UWWTau = sum(bsxfun(@times, ...
% $$$                         reshape(U(Mmv,n),[1,1,sum(Mmv)]), ...
% $$$                         WtauW(:,:,Mmv)), ...
% $$$                  3);

    % Distribution
    UWtauW = wsum(WtauW(:,:,Mmv),U(Mmv,n),3);
    CovX(:,:,n) = inv( speye(D) + UWtauW );
    X(:,n) = CovX(:,:,n) * ...
             ( tauW(Mmv,:)'*spdiag(Y(Mmv,n)) - WtauMu(Mmv,:)' ) * U(Mmv,n);

    % Expectations
    XX(:,:,n) = CovX(:,:,n) + X(:,n)*X(:,n)';
  end

  % KL-term: <log q(X)> - <log p(X)>
  KL_X = 0;
  for n=1:N
    KL_X = KL_X - gaussian_entropy(logdet_cov(CovX(:,:,n)), D);
    KL_X = KL_X - gaussian_logpdf(trace(XX(:,:,n)), ...
                                  0, ...
                                  0, ...
                                  0, ...
                                  D);
  end

  %%%%%%%%%%%%%%%%%
  %
  % W BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %
  % Update distributions and evaluate statistics
  %

  % Update q(W,Mu|tau)
  for m=1:M
    Nmv = Obs(m,:);

    UXX = wsum(XX(:,:,Nmv),U(m,Nmv),3);
    UX = X(:,Nmv)*U(m,Nmv)';
    UY = U(m,Nmv).*Y(m,Nmv);
    UYX = [UY*X(:,Nmv)', sum(UY)];

    % Distribution
    CovWMu(:,:,m) = inv(spdiag([alpha; beta]) + [UXX, UX; UX', sum(U(m,Nmv))]);
    W(m,:) = UYX*CovWMu(:,1:D,m);
    Mu(m) = UYX*CovWMu(:,end,m);
  end

  % Update q(tau)
  for m=1:M
    Nmv = Obs(m,:);
    a_tau(m) = p.a_tau + 0.5 * sum(Nmv);
    b_tau(m) = p.b_tau ...
        + 0.5 * (U(m,Nmv).*Y(m,Nmv))*Y(m,Nmv)' ...
        - 0.5 * [W(m,:),Mu(m)] * inv(CovWMu(:,:,m)) * [W(m,:),Mu(m)]';
  end
  if options.common_tau
    % Common tau
    a_tau(:) = p.a_tau + sum(a_tau-p.a_tau);
    b_tau(:) = p.b_tau + sum(b_tau-p.b_tau);
  end
  tau = a_tau./b_tau;
  logtau = psi(a_tau) - log(b_tau);
  for m=1:M
    % Expectations
    tauW(m,:) = tau(m) * W(m,:);
    tauMu(m) = tau(m) * Mu(m);
    WtauW(:,:,m) = tau(m)*W(m,:)'*W(m,:) + CovWMu(1:D,1:D,m);
    MutauMu(m) = tau(m)*Mu(m)^2 + CovWMu(end,end,m);
    WtauMu(m,:) = tau(m)*Mu(m)*W(m,:) + CovWMu(end,1:D,m);
  end

  % Update q(alpha)
  if (numel(options.update_alpha)==1 && iter >= options.update_alpha) || ...
        any(iter==options.update_alpha)
    a_alpha(:) = p.a_alpha + 0.5*M;
    b_alpha(:) = p.b_alpha + 0.5*diag(sum(WtauW, 3));
  end
  alpha = a_alpha ./ b_alpha;
  logalpha = psi(a_alpha) - log(b_alpha);

  % Update q(beta)
  if (numel(options.update_beta)==1 && iter >= options.update_beta) || ...
        any(iter==options.update_beta)
    a_beta = p.a_beta + 0.5*M;
    b_beta = p.b_beta + 0.5*sum(MutauMu);
  end
  beta = a_beta ./ b_beta;
  logbeta = psi(a_beta) - log(b_beta);

  %
  % Compute KL terms
  %

  % KL-term: <log q(W,Mu|tau)> - <log p(W|tau,alpha)> - <log p(Mu|tau,beta)>
  KL_WMu = 0;
  for m=1:M
    KL_WMu = KL_WMu - gaussian_entropy(-(D+1)*logtau(m) ...
                                       + logdet_cov(CovWMu(:,:,m)), ...
                                       D);
    KL_WMu = KL_WMu - gaussian_logpdf(diag(WtauW(:,:,m))'*alpha, ...
                                      0, ...
                                      0, ...
                                      -D*logtau(m) - sum(logalpha), ...
                                      D);
    KL_WMu = KL_WMu - gaussian_logpdf(diag(MutauMu(m))'*beta, ...
                                      0, ...
                                      0, ...
                                      -logtau(m) - sum(logbeta), ...
                                      1);
  end
% $$$   KL_W = 0;
% $$$   for m=1:M
% $$$     KL_W = KL_W - gaussian_entropy(-D*logtau(m) + logdet_cov(CovW(:,:,m)), D);
% $$$     KL_W = KL_W - gaussian_logpdf(diag(WtauW(:,:,m))'*w, ...
% $$$                                   0, ...
% $$$                                   0, ...
% $$$                                   -D*logtau(m) - sum(logw), ...
% $$$                                   D);
% $$$   end

  % KL-term: <log q(tau)> - <log p(tau)>
  KL_tau = 0;
  if options.common_tau
    KL_tau = KL_tau - gamma_entropy(a_tau(1), b_tau(1));
    KL_tau = KL_tau - gamma_logpdf(tau(1),logtau(1),p.a_tau,p.b_tau);
  else
    KL_tau = KL_tau - sum(gamma_entropy(a_tau, b_tau));
    KL_tau = KL_tau - sum(gamma_logpdf(tau,logtau,p.a_tau,p.b_tau));
  end

  % KL-term: <log q(alpha)> - <log p(alpha)>
  KL_alpha = 0;
  KL_alpha = KL_alpha - sum(gamma_entropy(a_alpha, b_alpha));
  KL_alpha = KL_alpha - sum(gamma_logpdf(alpha, logalpha, p.a_alpha, p.b_alpha));

  % KL-term: <log q(beta)> - <log p(beta)>
  KL_beta = 0;
  KL_beta = KL_beta - sum(gamma_entropy(a_beta, b_beta));
  KL_beta = KL_beta - sum(gamma_logpdf(beta, logbeta, p.a_beta, p.b_beta));

  %%%%%%%%%%%%%%%%%
  %
  % NOISE BLOCK
  %
  %%%%%%%%%%%%%%%%%

  %% UPDATE U AND NU
  switch options.robustness

   case 'none'
    %% Normal PCA / FA
    U(Obs) = 1;
    logU(Obs) = 0;
    nu(:) = inf;
    KL_U = 0;

   case 'multivariate-t'
    %% Joint Student-t for the dimensions (Archambaeu et al), except X
    %% independent of U, i.e., p(X) is Gaussian.

    % Evaluate squared errors
    for m=1:M
      Nmv = Obs(m,:);

      E2(m,Nmv) = tau(m)*Y(m,Nmv).^2 ...
          + mtraceprod(WtauW(:,:,m), XX(:,:,Nmv)) ...
          + MutauMu(m) ...
          - 2*Y(m,Nmv).*(tauW(m,:)*X(:,Nmv)) ...
          - 2*Y(m,Nmv).*tauMu(m) ...
          + 2*WtauMu(m,:)*X(:,Nmv);
    end

    % Update nu
    if (numel(options.update_nu)==1 && iter >= options.update_nu) ...
          || any(iter==options.update_nu)

      % Common nu
      nu(:) = t_ml(sum(E2,1), nu(1), sum(Obs,1));
 %     disp('Update nu');
    end

    % Update U
    a_U(1,:) = (nu(1) + sum(Obs,1)) / 2;
    b_U(1,:) = (nu(1) + sum(E2,1)) / 2;
    a_U(:,:) = repmat(a_U(1,:), [M,1]);
    b_U(:,:) = repmat(b_U(1,:), [M,1]);
    U(Obs) = a_U(Obs)./b_U(Obs);
    logU(Obs) = psi(a_U(Obs)) - log(b_U(Obs));

    % KL-term: <log q(U)> - <log p(U)>
    KL_U = 0;
    Nmv = sum(Obs,1) > 0;
    KL_U = KL_U - sum(gamma_entropy(a_U(1,Nmv),b_U(1,Nmv)));
    KL_U = KL_U - sum(gamma_logpdf(U(1,Nmv),logU(1,Nmv),nu(1)/2,nu(1)/2));

   case 'independent-t'
    %% Independent Student-t for each dimension (Luttinen et al)

    % Evaluate squared errors (this costs A LOT!)
    for m=1:M
      Nmv = Obs(m,:);

      E2(m,Nmv) = tau(m)*Y(m,Nmv).^2 ...
          + mtraceprod(WtauW(:,:,m), XX(:,:,Nmv)) ...
          + MutauMu(m) ...
          - 2*Y(m,Nmv).*(tauW(m,:)*X(:,Nmv)) ...
          - 2*Y(m,Nmv).*tauMu(m) ...
          + 2*WtauMu(m,:)*X(:,Nmv);
    end

    % Update nu
    if (numel(options.update_nu)==1 && iter >= options.update_nu) ...
          || any(iter==options.update_nu)
      if options.common_nu
        % Common nu
        nu(:) = t_ml(E2(Obs), nu(1));
      else
        for m=1:M
          % Separate nu
          if true
            % Type II ML
            nu(m) = t_ml(E2(m,Obs(m,:)), nu(m));
          else
            % EM ML
            nv = Obs(m,:);
            func = @(lognu) (-1-log(exp(lognu)/2)+psi(exp(lognu)/2)- ...
                             mean(logU(m,nv)-U(m,nv)));
            lognu = fzero(func, log(nu(m)));
            nu(m) = exp(lognu);
          end
        end
      end
%      disp('Update nu');
    end

    % Update U
    for m=1:M
      Nmv = Obs(m,:);
      a_U(m,Nmv) = (nu(m)+1) / 2;
      b_U(m,Nmv) = (nu(m) + E2(m,Nmv)) / 2;
% $$$     b_U(m,Nmv) = (nu(m) + E2(m,Nmv)*tau(m)) / 2;
    end
    U(Obs) = a_U(Obs)./b_U(Obs);
    logU(Obs) = psi(a_U(Obs)) - log(b_U(Obs));

    % KL-term: <log q(U)> - <log p(U)>
    KL_U = 0;
    KL_U = KL_U - sum(gamma_entropy(a_U(Obs),b_U(Obs)));
    for m=1:M
      Nmv = Obs(m,:);
      KL_U = KL_U - sum(gamma_logpdf(U(m,Nmv),logU(m,Nmv),nu(m)/2,nu(m)/2));
    end

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% Calculate lower bound of the log-likelihood

  monitor_loglikelihood = true;

  if monitor_loglikelihood

    % Cost from Y

    % We can take advantage of the linearity of gaussian_logpdf by summing
    % before the function call:
    tauYh = bsxfun(@plus, tauW*X, tauMu);
    y_invCov_y = (tau(ObsM).*U(Obs))'*(Y(Obs).^2);
    y_invCov_mu = U(Obs)'*(Y(Obs).*tauYh(Obs));
    mu_invCov_mu = 0;
    for m=1:M
      Nmv = Obs(m,:);
      mu_invCov_mu = mu_invCov_mu ...
          + traceprod(WtauW(:,:,m),wsum(XX(:,:,Nmv),U(m,Nmv),3)) ...
          + sum(U(m,Nmv)) * MutauMu(m) ...
          + 2*WtauMu(m,:)*(X(:,Nmv)*U(m,Nmv)');
    end
    logdet_Cov = - sum(logtau(ObsM)) - sum(logU(Obs));
    dim_y = sum(Obs(:));

    logpdf_Y = gaussian_logpdf(y_invCov_y, ...
                               y_invCov_mu, ...
                               mu_invCov_mu, ...
                               logdet_Cov, ...
                               dim_y);

    old_logP = logP;

    %    logpdf_Y

% $$$     KL_U
% $$$     KL_X
% $$$     KL_tau
% $$$     KL_W
% $$$     KL_w
    logP = logpdf_Y - KL_U - KL_X - KL_tau - KL_WMu - KL_alpha - KL_beta;

% $$$     % DEBUG STUFF
% $$$     kl_Y(iter) = logpdf_Y;
% $$$     kl_U(iter) = KL_U;
% $$$     kl_X(iter) = KL_X;
% $$$     kl_W(iter) = KL_W;
% $$$     kl_tau(iter) = KL_tau;
% $$$     kl_w(iter) = KL_w;

    loglikelihood(iter) = logP;
  else
    loglikelihood(iter) = nan;
  end

  time(iter) = cputime - start_time;

  % Debugging: Check that the bound really improves
  if iter > 1 && loglikelihood(iter) < loglikelihood(iter-1)
    logP_diff = (loglikelihood(iter-1)-loglikelihood(iter))/ ...
        abs(loglikelihood(iter-1));
    warmsg = sprintf(['Lower bound decreased %.2e percents. Bug or numerical ' ...
                      'inaccuracy?'], logP_diff);
    warning(warmsg);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% Monitoring stuff

  % Show progress
  fprintf('Step %d: loglikelihood=%e (%.3f seconds)\n', ...
          iter, loglikelihood(iter), cputime-t);

  % Check whether to save the results
  if (now-lastsave)*3600*24 >= options.autosavetime
    fprintf('Saving to %s...', options.autosavefile);
    save(options.autosavefile, 'W','CovW','w','a_w','b_w','X','CovX', ...
         'tau', 'a_tau','b_tau','U','a_U','b_U','nu', 'loglikelihood', ...
         'time', 'iter', 'options');
    lastsave = now;
    fprintf(' done.\n');
  end

  % Transformations for speeding up
  if (numel(options.rotate)==1 && iter>=options.rotate && options.rotate>=1) ...
        || any(iter==options.rotate)
    %orthogonalize();
    rotation2();
  end

end

% $$$ figure
% $$$ kl = [kl_Y(:), kl_U(:), kl_X(:), kl_W(:), kl_tau(:), kl_w(:)];
% $$$ kl = kl(6:end,:);
% $$$ kl = eminus(kl,mean(kl,1));
% $$$ plot(kl)

% Results as a struct
Q.W = W;
Q.CovWMu = CovWMu;
Q.alpha = alpha;
Q.a_alpha = a_alpha;
Q.b_alpha = b_alpha;
Q.Mu = Mu;
Q.beta = beta;
Q.a_beta = beta;
Q.b_beta = b_beta;
Q.X = X;
Q.CovX = CovX;
Q.tau = tau;
Q.a_tau = a_tau;
Q.b_tau = b_tau;
Q.U = U;
Q.a_U = a_U;
Q.b_U = b_U;
Q.nu = nu;
Q.loglikelihood = loglikelihood(1:iter);
Q.time = time(1:iter);
Q.iter = iter;
Q.options = options;

%%%%%%%%%%%%%%%%%%%%%%
%% NESTED FUNCTIONS

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

function rotation2()

%alpha_before = alpha

% Rotates W and X, and moves bias from X to Mu.

%Y0 = bsxfun(@plus, W*X, Mu);

sumX = sum(X,2);
sumXX = [sum(XX,3), sumX; sumX', N]; % extended model: [X;1]*[X;1]'
sumWtauW = sum(WtauW,3);
sumMutauMu = sum(MutauMu);
sumWtauMu = sum(WtauMu,1);
sumWMutauWMu = [sumWtauW, sumWtauMu'; sumWtauMu, sumMutauMu];

%diag_wtauw = diag(sum(WtauW,3))

%
% Optimize the rotation
%

R0 = eye([D,D+1]);
%mycheckgrad(@cost, R0(:), 1e-6);
r = minimize(R0(:), @cost, 50);
%r(:) = randn(size(r)); % DEBUG !!!
R = [reshape(r,[D,D+1]); zeros(1,D), 1];
%R
%R = eye(D+1);% DEBUG!!!
invR = inv(R);

%
% Apply the rotation
%

% W and Mu
tauWMu = [tauW,tauMu]*R;
tauW = tauWMu(:,1:D);
tauMu = tauWMu(:,end);
for m=1:M
  Z = [WtauW(:,:,m), WtauMu(m,:)'; WtauMu(m,:), MutauMu(m)];
  % Expectations
  WtauW(:,:,m) = R(:,1:D)'*Z*R(:,1:D);
  MutauMu(m) = R(:,end)'*Z*R(:,end);
  WtauMu(m,:) = R(:,end)'*Z*R(:,1:D);
end

% X
X = invR(1:D,:)*[X;ones(1,N)];
for n=1:N
  XX(:,:,n) = invR(1:D,:)*[XX(:,:,n),X(:,n);X(:,n)',1]*invR(1:D,:)';
end

% alpha and beta
%diag_wtauw = diag(sum(WtauW,3))
b_alpha = p.b_alpha + 0.5*diag(sum(WtauW,3));
alpha = a_alpha ./ b_alpha;
b_beta = p.b_beta + 0.5*sum(MutauMu);
beta = a_beta ./ b_beta;

%alpha_after = alpha

%meanX = mean(X,2)
%sumXX = sum(XX,3) / N
%sumWW = sum(WtauW,3) / M

%Y1 = bsxfun(@plus, W*X, Mu);

%difference = sqrt(mean((Y1(:)-Y0(:)).^2))

%error('jou')

  function [f,df] = cost(r)

  R = [reshape(r,[D,D+1]); zeros(1,D), 1];
  R(end) = 1;

  invR = inv(R);
  invRR = inv(R*R');
  logdetR = logdet(R);

  % Transform W and Mu
  WW_R = sumWMutauWMu*R;
  R_WW_R = R'*WW_R;

  % Transform q(alpha) and q(beta)
  % (concatenate alpha and beta to one vector for simplicity)
  z = diag(R_WW_R);
  r_b_alpha = [repmat(p.b_alpha,D,1);p.b_beta] + 0.5*z;
  r_alpha = [a_alpha;a_beta] ./ r_b_alpha;
  r_logalpha = - log(r_b_alpha); % up to a constant
  grad_b = WW_R;
  grad_logb = grad_b * diag(1./r_b_alpha);
  grad_logalpha = -grad_logb;
  grad_alpha = grad_logalpha * diag(r_alpha);

  % Transform X

  % Cost from W and Mu
  %
  % TODO: There is something wrong with this!!
  logdet_Cov = 2*M*logdetR;
  x_invCov0_x = traceprod(spdiag(r_alpha),R_WW_R);
  x_invCov0_mu = 0;
  mu_invCov0_mu = 0;
  logdet_Cov0 = -M * sum(r_logalpha);
  grad_logdet_Cov = 2*M*invR';
  grad_x_invCov0_x = 2*WW_R*spdiag(r_alpha) ...
      + grad_alpha * diag(diag(R_WW_R));
  grad_x_invCov0_mu = 0;
  grad_mu_invCov0_mu = 0;
  grad_logdet_Cov0 = -M * grad_logalpha;
  [KL_W,dKL_W] = vb_rotationcost_gaussian(logdet_Cov, ...
                                          x_invCov0_x, ...
                                          x_invCov0_mu, ...
                                          mu_invCov0_mu, ...
                                          logdet_Cov0, ...
                                          grad_logdet_Cov, ...
                                          grad_x_invCov0_x, ...
                                          grad_x_invCov0_mu, ...
                                          grad_mu_invCov0_mu, ...
                                          grad_logdet_Cov0);
  %KL_W = 0;
  %dKL_W = 0;

  % Cost from alpha and beta
  prior_a = [repmat(p.a_alpha,D,1);p.a_beta];
  prior_b = [repmat(p.b_alpha,D,1);p.b_beta];
  post_a = [a_alpha;a_beta];
  A0_logB = prior_a'*log(r_b_alpha);
  A_B0_invB = prior_b'*r_alpha;
  grad_A0_logB = grad_logb * diag(prior_a);
  grad_A_B0_invB = grad_logalpha * diag(prior_b);
  [KL_alpha,dKL_alpha] = vb_rotationcost_gamma(A0_logB,      ...
                                               A_B0_invB,    ...
                                               grad_A0_logB, ...
                                               grad_A_B0_invB);
  %KL_alpha = 0;
  %dKL_alpha = 0;

  % Cost from X
  logdet_Cov = -2*N*logdetR;
  x_invCov0_x = traceprod(invR(1:D,:)'*invR(1:D,:),sumXX);
  x_invCov0_mu = 0;
  mu_invCov0_mu = 0;
  logdet_Cov0 = 0;
  grad_logdet_Cov = -2*N*invR';
  grad_x_invCov0_x = -2*invRR*sumXX*invR';
  grad_x_invCov0_mu = 0;
  grad_mu_invCov0_mu = 0;
  grad_logdet_Cov0 = 0;
  [KL_X,dKL_X] = vb_rotationcost_gaussian(logdet_Cov, ...
                                          x_invCov0_x, ...
                                          x_invCov0_mu, ...
                                          mu_invCov0_mu, ...
                                          logdet_Cov0, ...
                                          grad_logdet_Cov, ...
                                          grad_x_invCov0_x, ...
                                          grad_x_invCov0_mu, ...
                                          grad_mu_invCov0_mu, ...
                                          grad_logdet_Cov0);
  %KL_X = 0;
  %dKL_X = 0;

  % Vectorize results
  f = KL_W + KL_alpha + KL_X;
  df = dKL_W + dKL_alpha + dKL_X;
  df = df(1:D,:);
  df = df(:);

  end

end

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

function orthogonalize

  %% Move bias (approximate: mean of X to zero)
% $$$   dmu = mean(X,2);
% $$$   X = X - repmat(dmu,1,N);
% $$$   tauMu = tauMu + tauW*dmu;
% $$$   WtauMu = WtauMu ???
% $$$   MutauMu = ???

  %% Rotate W and X

  Yh0 = tauW*X;

  % Whitening of X
  [Vx,D2x] = svd(sum(XX,3)/N);
  Dx = spdiag(sqrt(diag(D2x))); % convert to sparse diagonal form
  Qx = Dx \ Vx';
  Qw = full(Vx * Dx); % funny: could be sparse if size(Dx)=[1 1] ??
  tauW = tauW * Qw;
  WtauMu = WtauMu * Qw;
  for m=1:M
%    CovW(:,:,m) = Qw' * CovW(:,:,m) * Qw;
    WtauW(:,:,m) = Qw' * WtauW(:,:,m) * Qw;
% $$$     WW(:,:,m) = W(m,:)'*W(m,:) + CovW(:,:,m);
  end

  % Orthogonalization of W
  [Vw,Dw] = svd(sum(WtauW,3)/M);
% $$$   [Vw,Dw] = svd(sum(WW,3)/M);
  Qx = Vw' * Qx;
  Qw = Vw;
%  W = W * Qw;
  tauW = tauW * Qw;
  WtauMu = WtauMu * Qw;
  for m=1:M
%    CovW(:,:,m) = Qw' * CovW(:,:,m) * Qw;
    WtauW(:,:,m) = Qw' * WtauW(:,:,m) * Qw; %W(m,:)'*W(m,:) + CovW(:,:,m);
% $$$     WW(:,:,m) = W(m,:)'*W(m,:) + CovW(:,:,m);
  end

  % Apply rotations to X
  X = Qx * X;
  for n = 1:N
    CovX(:,:,n) = Qx * CovX(:,:,n) * Qx';
    XX(:,:,n) = X(:,n)*X(:,n)' + CovX(:,:,n);
  end

  Yh1 = tauW*X;

% $$$   diff_in_orth = norm(Yh1-Yh0)
% $$$
% $$$   sumWtauW = sum(WtauW,3)
% $$$   sumXX = sum(XX,3) / N

end

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

function initialize()

  disp('Initialize variables..')

  % Initialize X
  X = nan * zeros(D,N);
  CovX = nan*zeros([D,D,N]); % repmat(eye(D),[1,1,N]);

  % Initialize q(alpha)
  a_alpha = nan * ones(D,1);
  b_alpha = nan * ones(D,1);
  alpha = 1e-3 * ones(D,1);
  logalpha = nan * ones(D,1);

  % Initialize q(beta)
  a_beta = nan;
  b_beta = nan;
  beta = 1e-3;
  logbeta = nan;

  % Initialize W and Mu
  W = randn(M,D);
  Mu = zeros(M,1);
  CovWMu = repmat(eye(D+1),[1,1,M]);

  % Initialize tau
  a_tau = nan * ones(M,1);
  b_tau = nan * ones(M,1);
  tau = 1e1 * ones(M,1);
  logtau = nan * ones(M,1);

  % Initialize nu and q(U)
  nu = 1 * ones(M,1);
  a_U = nan * ones(M,N);
  b_U = nan * ones(M,N);
  U = ones(M,N);
  logU = nan * zeros(M,N);

  % Initialize log likelihood
  loglikelihood = -Inf;

  % Initialize mean squared errors < (y-wx-m)^2 >
  E2 = zeros(size(Y));

  % Use given initialization (size matching is checked by using ':' )
  if isstruct(options.init)
    % X
    if isfield(options.init, 'X')
      disp('Custom initialization for X.');
      warning(['Currently, X is updated first, so given initialization has ' ...
               'no effect.']);
      X(:,:) = options.init.X;
    end
    if isfield(options.init, 'CovX')
      disp('Custom initialization for CovX.');
      warning(['Currently, X is updated first, so given initialization has ' ...
               'no effect.']);
      CovX(:,:,:) = options.init.CovX;
    end
    % W and Mu
    if isfield(options.init, 'W')
      disp('Custom initialization for W.');
      W(:,:) = options.init.W;
    end
    if isfield(options.init, 'Mu')
      disp('Custom initialization for Mu.');
      Mu(:,:) = options.init.Mu;
    end
    if isfield(options.init, 'CovWMu')
      disp('Custom initialization for CovWMu.');
      CovWMu(:,:,:) = options.init.CovWMu;
    end
    % alpha
    if isfield(options.init, 'alpha')
      disp('Custom initialization for alpha.');
      alpha(:) = options.init.alpha;
    end
% $$$     if isfield(options.init, 'a_alpha')
% $$$       disp('Custom initialization for a_alpha.');
% $$$       a_alpha(:) = options.init.a_alpha;
% $$$     end
% $$$     if isfield(options.init, 'b_alpha')
% $$$       disp('Custom initialization for b_alpha.');
% $$$       b_alpha(:) = options.init.b_alpha;
% $$$     end
    % beta
    if isfield(options.init, 'beta')
      disp('Custom initialization for beta.');
      beta(:) = options.init.beta;
    end
% $$$     if isfield(options.init, 'a_beta')
% $$$       disp('Custom initialization for a_beta.');
% $$$       a_beta(:) = options.init.a_beta;
% $$$     end
% $$$     if isfield(options.init, 'b_beta')
% $$$       disp('Custom initialization for b_beta.');
% $$$       b_beta(:) = options.init.b_beta;
% $$$     end
    % tau
    if isfield(options.init, 'tau')
      disp('Custom initialization for tau.');
      tau(:) = options.init.tau;
    end
% $$$     if isfield(options.init, 'a_tau')
% $$$       disp('Custom initialization for a_tau.');
% $$$       a_tau(:) = options.init.a_tau;
% $$$     end
% $$$     if isfield(options.init, 'b_tau')
% $$$       disp('Custom initialization for b_tau.');
% $$$       b_tau(:) = options.init.b_tau;
% $$$     end
    % nu
    if isfield(options.init, 'nu')
      disp('Custom initialization for nu.');
      nu(:) = options.init.nu;
    end
    % U
    if isfield(options.init, 'U')
      disp('Custom initialization for U.');
      U(:,:) = options.init.U;
    end
  end

  % Initialize second moments for X
  XX = zeros(D,D,N);
  for n=1:N
    XX(:,:,n) = X(:,n)*X(:,n)' + CovX(:,:,n);
  end

  % Initialize expectations for W and Mu
  tauW = zeros(M,D);
  tauMu = zeros(M,1);
  WtauW = zeros(D,D,M);
  MutauMu = zeros(M,1);
  WtauMu = zeros(M,D);
  for m=1:M
    tauW(m,:) = tau(m) * W(m,:);
    tauMu(m) = tau(m) * Mu(m);
    WtauW(:,:,m) = tau(m)*W(m,:)'*W(m,:) + CovWMu(1:D,1:D,m);
    MutauMu(m) = tau(m)*Mu(m)^2 + CovWMu(end,end,m);
    WtauMu(m,:) = tau(m)*Mu(m)*W(m,:) + CovWMu(end,1:D,m);
  end

end

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

function set_priors()

  % Default prior
  p.a_alpha = 1e-5;
  p.b_alpha = 1e-5;
  p.a_beta = 1e-5;
  p.b_beta = 1e-5;
  p.a_tau = 1e-5;
  p.b_tau = 1e-5;

  % Custom prior
  if isstruct(options.prior)

    % Prior for alpha
    if isfield(options.prior, 'a_alpha')
      p.a_alpha(1) = options.prior.a_alpha;
      disp('Custom prior parameter a_alpha');
    end
    if isfield(options.prior, 'b_alpha')
      p.b_alpha(1) = options.prior.b_alpha;
      disp('Custom prior parameter b_alpha');
    end

    % Prior for beta
    if isfield(options.prior, 'a_beta')
      p.a_beta(1) = options.prior.a_beta;
      disp('Custom prior parameter a_beta');
    end
    if isfield(options.prior, 'b_beta')
      p.b_beta(1) = options.prior.b_beta;
      disp('Custom prior parameter b_beta');
    end

    % Prior for tau
    if isfield(options.prior, 'a_tau')
      p.a_tau(1) = options.prior.a_tau
      disp('Custom prior parameter a_tau');
    end
    if isfield(options.prior, 'b_tau')
      p.b_tau(1) = options.prior.b_tau
      disp('Custom prior parameter b_tau');
    end

  end

end

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

function rotate

% I am not absolutely sure that this is bug free

% $$$ D = size(XX,1);
% $$$ N = size(XX,3);
% $$$ M = size(CC,3);

% R should be non-singular because we need to take log(abs(det(R)))

% X_X_ = sum(XX(:,:,1:(N-1)),3) + x0x0;
% CovA = 1/D * (sum(AA,3) - A'*A);
% $$$ Psi = sum(XX,3) - 2*A*sum(X_X,3) + A*X_X_*A' + traceprod(CovA,X_X_)*eye(D);

% Start with identity matrix which does not make the results worse
R = eye(D);

r = minimize(R(:),@cost,100);
R = reshape(r,[D,D]);

% Compute new statistics
for n=1:N
  XX(:,:,n) = R \ XX(:,:,n) / R';
  X_X(:,:,n) = R \ X_X(:,:,n) / R';
end
x0x0 = R \ x0x0 / R';
A = R \ A * R;
CovA = 1/D * trace(inv(R*R')) * R'*CovA*R;
for d=1:D
  AA(:,:,d) = A(d,:)'*A(d,:) + CovA;
end
C = C*R;
for m=1:M
  CC(:,:,m) = R'*CC(:,:,m)*R;
end

I = eye(D);
Rtmp = randn(D);

% Compute new hyperparameters here or after this function!

  function [f,df] = cost(x)

    R = reshape(x,[D,D]);
    [f,df] = bound(R);
    f = -f;
    df = -df(:);

  end

  function [l,dl] = bound(R)

  % Use QR to evaluate these more efficiently?
  RR = R'*R;
  invR = inv(R); % inv??
  invRR = inv(R*R'); % inv??
  logdetR = logdet(R);
  trinvRR = trace(invRR);

  Psi = sum(XX,3)*invR'/p.Cov_X - 2*inv(p.Cov_X)*sum(p.mu_X'*X,2);

  % A_invRR_A = A'*invRR*A + trinvRR*CovA;
  % diag_RA_invRR_AR = diag(R'*A_invRR_A*R);
  diagRCCR = diag(R'*sum(WtauW,3)*R); % not efficient

  % Bound terms
  lpX = -0.5 * traceprod(invRR,Psi);
  lqX = N * logdetR;
  lpA = 0; %-0.5 * D * sum(log(diag_RA_invRR_AR));
  lqA = 0; %-0.5*D*logabsdet(trinvRR*RR);
  lpC = -0.5 * M * sum(log(diagRCCR));
  lqC = -M * logdetR;

  % Bound
  l = lpX + lpA + lpC - lqX - lqA - lqC;

  % Derivative terms
  dlpX = invR * Psi * invRR;
  dlqX = N * invR;
  dlpA = 0; %-D*(diag(1./diag_RA_invRR_AR)*R'*A_invRR_A) ...
         %+ D*invR*A*R*diag(1./diag_RA_invRR_AR)*R'*A'*invRR ...
         %+ D*traceprod(diag(1./diag_RA_invRR_AR),R'*CovA*R)*invR*invRR;
  dlqA = 0; %-D * invR + D^2 * invR*invRR / trinvRR;
  dlpC = -M * (diag(1./diagRCCR)*R'*sum(CC,3));
  dlqC = -M * invR;

  % Derivative
  dl = (dlpX + dlpA + dlpC - dlqX - dlqA - dlqC)';
  end

end

end


function robot = parse_urdf(file)
% Loading file from urdf
% The function is tailored for UR robots: serial robots with 6 DOF
% Modify it if you have  a robot with different degrees of freedom
robot = xml2struct(file);
no_dof = 6;

% Extracting parameters of the robot
for i = 1:no_dof
    % axis of rotation of a joint i in coordinate system of joint i
    axis_of_rot = str2num(robot.robot.joint{i}.axis.Attributes.xyz)';
    % mass of link (i+1) because joint i rotates link (i+1) as the numbering of
    % links starts from base link that it not moving
    link_mass = str2double(robot.robot.link{i+1}.inertial.mass.Attributes.value);
    % poistion of the com in frame attached to link
    com_pos = str2num(robot.robot.link{i+1}.inertial.origin.Attributes.xyz)';
    com_vec2mat = vec2skewSymMat(com_pos);
    % inertial parameters of the link expressed in coordinate system attached
    % the center of mass.
    ixx = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixx);
    ixy = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixy);
    ixz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.ixz);
    iyy = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.iyy);
    iyz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.iyz);
    izz = str2double(robot.robot.link{i+1}.inertial.inertia.Attributes.izz);
    % the inertia tensor wrt the frame oriented as the body frame and with the
    % origin in the COM
    link_inertia = [ixx, ixy, ixz; ixy, iyy iyz; ixz, iyz, izz];
    % manipulator regressor
    robot.m(i) = link_mass;
    robot.k(:,i) = axis_of_rot;
    robot.r_com(:,i) = com_pos;
    robot.I(:,:,i) = link_inertia;
    robot.h(:,i) = link_mass*com_pos;
    robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
                            link_mass*com_vec2mat*com_vec2mat);
    robot.pi(:,i) = [robot.I_vec(:,i); robot.h(:,i); robot.m(i)];
end


function [coherogram,phase,t,f] = MTCoherogram(lfp1,lfp2,varargin)

%MTCoherogram - Compute LFP coherogram by multi-taper estimation.
%
%  USAGE
%
%    [coherogram,phase,t,f] = MTCoherogram(lfp1,lfp2,<options>)
%
%    lfp1,lfp2      wide-band LFPs (one channel each).
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'frequency'   sampling rate (in Hz) (default = from timestamps if
%                   available, otherwise 1250Hz)
%     'range'       frequency range (in Hz) (default = all)
%     'window'      duration (in s) of the time window (default = 5)
%     'overlap'     overlap between successive windows (default = window/2)
%     'step'        step between successive windows (default = window/2)
%     'tapers'      relative resolution and order of the tapers [NW K]
%                   (default = [3 5])
%     'pad'         FFT padding (see help for cohgramc) (default = 0)
%     'show'        plot results (default = 'off')
%     'cutoffs'     cutoff values for color plot (default = [0 1])
%    =========================================================================
%
%  NOTES
%
%    The LFP can be provided either as a time stamped matrix (list of time-voltage
%    pairs), or as a voltage vector - in which case the frequency must be specified.
%
%    The time displacement between successive short time coherences can be supplied
%    either as a 'step' (explicit time difference) or as an 'overlap' (between
%    successive time windows).
%
%  OUTPUT
%
%    coherogram     coherogram magnitude
%    phase          coherogram phase
%    t              time bins
%    f              frequency bins
%
%  DEPENDENCIES
%
%    This function requires the chronux toolbox.
%
%  SEE
%
%    See also MTCoherence, MTSpectrum, MTSpectrogram, PlotColorMap.

% Copyright (C) 2010-2014 by Michaël Zugaro
%
% 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.
warning('This function has been deprecated, and will be removed in the future. Try using bz_MTCoherogram.m')
% Make sure chronux is installed and functional
CheckChronux('cohgramc');

% Defaults
f = 1250;
frequency = [];
window = 5;
range = [];
overlap = [];
step = [];
show = 'off';
tapers = [3 5];
pad = 0;
cutoffs = [0 1];

% Check number of parameters
if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help MTCoherogram'' for details).');
end

% Check parameter sizes
if size(lfp1,2) ~= 1 && size(lfp1,2) ~= 2,
	error('Parameter ''lfp1'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end
if size(lfp2,2) ~= 1 && size(lfp2,2) ~= 2,
	error('Parameter ''lfp2'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help MTCoherogram'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'frequency',
			frequency = varargin{i+1};
			if ~isdscalar(frequency,'>0'),
				error('Incorrect value for property ''frequency'' (type ''help MTCoherogram'' for details).');
			end
		case 'range',
			range = varargin{i+1};
			if ~isdvector(range,'#2','<','>=0'),
				error('Incorrect value for property ''range'' (type ''help MTCoherogram'' for details).');
			end
		case 'window',
			window = varargin{i+1};
			if ~isdscalar(window,'>0'),
				error('Incorrect value for property ''window'' (type ''help MTCoherogram'' for details).');
			end
		case 'overlap',
			overlap = varargin{i+1};
			if ~isdscalar(overlap,'>0'),
				error('Incorrect value for property ''overlap'' (type ''help MTCoherogram'' for details).');
			end
		case 'step',
			step = varargin{i+1};
			if ~isdscalar(step,'>0'),
				error('Incorrect value for property ''step'' (type ''help MTCoherogram'' for details).');
			end
		case 'tapers',
			tapers = varargin{i+1};
			if ~isivector(tapers,'#2','>0'),
				error('Incorrect value for property ''tapers'' (type ''help MTCoherogram'' for details).');
			end
		case 'pad',
			pad = varargin{i+1};
			if ~isiscalar(pad,'>-1'),
				error('Incorrect value for property ''pad'' (type ''help MTCoherogram'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring_FMAT(show,'on','off'),
				error('Incorrect value for property ''show'' (type ''help MTCoherogram'' for details).');
			end
		case 'cutoffs',
			cutoffs = varargin{i+1};
			if ~isdvector(cutoffs,'#2','>=0','<'),
				error('Incorrect value for property ''cutoffs'' (type ''help MTCoherogram'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help MTCoherogram'' for details).']);
	end
end

% Determine LFP frequency
if isempty(frequency),
	if size(lfp1,2) == 2,
		frequency = 1/median(diff(lfp1(:,1)));
	else
		frequency = f;
	end
end

% Determine step/overlap
if isempty(step),
	if isempty(overlap),
		overlap = window/2;
	end
else
	if isempty(overlap),
		overlap = window-step;
	elseif overlap ~= window-step,
		error('Incompatible ''step'' and ''overlap'' parameters (type ''help MTCoherogram'' for details).');
	end
end

% Compute and plot coherogram
parameters.Fs = frequency;
if ~isempty(range), parameters.fpass = range; end
parameters.tapers = tapers;
parameters.pad = pad;
[coherogram,phase,~,~,~,t,f] = cohgramc(lfp1(:,2),lfp2(:,2),[window window-overlap],parameters);
t = t'+lfp1(1,1);
f = f';
coherogram = coherogram';
% coherogram = permute(coherogram,[2 1 3]);  % Previous code by Gabrielle Girardeau, keep it around just in case
phase = phase';
if strcmp(lower(show),'on'),
  figure;hold on;
  subplot(2,1,1);
  PlotColorMap(coherogram,'x',t,'y',f,'cutoffs',cutoffs,'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Amplitude');
  subplot(2,1,2);
  PlotColorMap(phase,'x',t,'y',f,'cutoffs',[-pi pi],'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Phase');
end


%MDL_QUADCOPTER Dynamic parameters for a quadrotor.
%
% MDL_QUADCOPTER is a script creates the workspace variable quad which
% describes the dynamic characterstics of a quadrotor flying robot.
%
% Properties::
%
% This is a structure with the following elements:
%
% nrotors   Number of rotors (1x1)
% J         Flyer rotational inertia matrix (3x3)
% h         Height of rotors above CoG (1x1)
% d         Length of flyer arms (1x1)
% nb        Number of blades per rotor (1x1)
% r         Rotor radius (1x1)
% c         Blade chord (1x1)
% e         Flapping hinge offset (1x1)
% Mb        Rotor blade mass (1x1)
% Mc        Estimated hub clamp mass (1x1)
% ec        Blade root clamp displacement (1x1)
% Ib        Rotor blade rotational inertia (1x1)
% Ic        Estimated root clamp inertia (1x1)
% mb        Static blade moment (1x1)
% Ir        Total rotor inertia (1x1)
% Ct        Non-dim. thrust coefficient (1x1)
% Cq        Non-dim. torque coefficient (1x1)
% sigma     Rotor solidity ratio (1x1)
% thetat    Blade tip angle (1x1)
% theta0    Blade root angle (1x1)
% theta1    Blade twist angle (1x1)
% theta75   3/4 blade angle (1x1)
% thetai    Blade ideal root approximation (1x1)
% a         Lift slope gradient (1x1)
% A         Rotor disc area (1x1)
% gamma     Lock number (1x1)
%
%
% Notes::
% - SI units are used.
%
% References::
% - Design, Construction and Control of a Large Quadrotor micro air vehicle.
%   P.Pounds, PhD thesis,
%   Australian National University, 2007.
%   http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf
% - This is a heavy lift quadrotor
%
% See also sl_quadrotor.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

quadrotor.nrotors = 4;                %   4 rotors
quadrotor.g = 9.81;                   %   g       Gravity                             1x1
quadrotor.rho = 1.184;                %   rho     Density of air                      1x1
quadrotor.muv = 1.5e-5;               %   muv     Viscosity of air                    1x1

% Airframe
quadrotor.M = 4;                      %   M       Mass                                1x1
% Ixx = 0.082;
% Iyy = 0.082;
% Izz = 0.149;%0.160;
quadrotor.J = diag([0.082 0.082 0.149]);    %   I       Flyer rotational inertia matrix     3x3

quadrotor.h = -0.007;                 %   h       Height of rotors above CoG          1x1
quadrotor.d = 0.315;                  %   d       Length of flyer arms                1x1

%Rotor
quadrotor.nb = 2;                      %   b       Number of blades per rotor          1x1
quadrotor.r = 0.165;                  %   r       Rotor radius                        1x1

quadrotor.c = 0.018;                  %   c       Blade chord                         1x1

quadrotor.e = 0.0;                    %   e       Flapping hinge offset               1x1
quadrotor.Mb = 0.005;                 %   Mb      Rotor blade mass                    1x1
quadrotor.Mc = 0.010;                 %   Mc      Estimated hub clamp mass            1x1
quadrotor.ec = 0.004;                 %   ec      Blade root clamp displacement       1x1
quadrotor.Ib = quadrotor.Mb*(quadrotor.r-quadrotor.ec)^2/4 ;        %   Ib      Rotor blade rotational inertia      1x1
quadrotor.Ic = quadrotor.Mc*(quadrotor.ec)^2/4;           %   Ic      Estimated root clamp inertia        1x1
quadrotor.mb = quadrotor.g*(quadrotor.Mc*quadrotor.ec/2+quadrotor.Mb*quadrotor.r/2);    %   mb      Static blade moment                 1x1
quadrotor.Ir = quadrotor.nb*(quadrotor.Ib+quadrotor.Ic);             %   Ir      Total rotor inertia                 1x1

quadrotor.Ct = 0.0048;                %   Ct      Non-dim. thrust coefficient         1x1
quadrotor.Cq = quadrotor.Ct*sqrt(quadrotor.Ct/2);         %   Cq      Non-dim. torque coefficient         1x1

quadrotor.sigma = quadrotor.c*quadrotor.nb/(pi*quadrotor.r);         %   sigma   Rotor solidity ratio                1x1
quadrotor.thetat = 6.8*(pi/180);      %   thetat  Blade tip angle                     1x1
quadrotor.theta0 = 14.6*(pi/180);     %   theta0  Blade root angle                    1x1
quadrotor.theta1 = quadrotor.thetat - quadrotor.theta0;   %   theta1  Blade twist angle                   1x1
quadrotor.theta75 = quadrotor.theta0 + 0.75*quadrotor.theta1;%   theta76 3/4 blade angle                     1x1
quadrotor.thetai = quadrotor.thetat*(quadrotor.r/quadrotor.e);      %   thetai  Blade ideal root approximation      1x1
quadrotor.a = 5.5;                    %   a       Lift slope gradient                 1x1

% derived constants
quadrotor.A = pi*quadrotor.r^2;                 %   A       Rotor disc area                     1x1
quadrotor.gamma = quadrotor.rho*quadrotor.a*quadrotor.c*quadrotor.r^4/(quadrotor.Ib+quadrotor.Ic);%   gamma   Lock number                         1x1

quadrotor.b = quadrotor.Ct*quadrotor.rho*quadrotor.A*quadrotor.r^2; % T = b w^2
quadrotor.k = quadrotor.Cq*quadrotor.rho*quadrotor.A*quadrotor.r^3; % Q = k w^2

quadrotor.verbose = false;