function [y]=tt_exp(x, eps, varargin)
%Computation of the pointwise exponential in TT format
%   [Y]=TT_EXP(X,EPS,VARARGIN) This function computes pointwise exponential
%   using scaling and squaring method of the TT-vector X. EPS is the
%   accuracy parameter, varargins:
%       N is the number of summand for the local Taylor
%           series (N=10 by default, usually enough).
%       RMAX is the TT-rank bound.

% nrm = norm(x);
if (isa(x, 'tt_tensor'))
    nrm = tt_max_abs(x);
else
    nrm = tt_max_abs(qtttucker_to_linqtt(x, eps));
end;
n0 = floor(max(log2(nrm), 0))+1;
x = x./(2^n0);
if (isa(x, 'tt_tensor'))
    ons = tt_ones(x.n);
else
    ons = [];
    for i=1:(x.core.d)
        curons = tt_ones(x.tuck{i}.n);
        curons = qtt_tucker(curons, x.tuck{i}.d, eps);
        ons = kron(ons, curons);
    end;
end;
y = ons;

N = 10;
hdm = 'svd';
rmax = Inf;
epst = eps;
while (length(varargin)==1)
    varargin = varargin{1};
end;
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'n'
            N=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        case 'epst'
            epst=varargin{i+1};
        case 'hdm'
            hdm=varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

for k=(N-1):-1:1
% for k=1:N-1
    y=ons+(y.*x)/k;
    y=round(y,epst,rmax);
end

for k=1:n0
    if (strcmp(hdm, 'svd'))
        y=round(y.*y,eps*(0.5^(n0-k)),rmax);
        fprintf('squaring %d\n', k);
    else
        if (isa(x, 'tt_tensor'))
%             y = mvk3(diag(y), y, eps, 'nswp', 20, 'kickrank', 2);
            y = tt_mvk4(diag(y), y, eps, 'nswp', 20);
        else
%             y = mvrk(diag(y), y, eps, 'nswp', 20, 'kickrank', 2);
            y = mvrk2(diag(y), y, eps, 'nswp', 20);
        end;
    end;
end

end


function [yNext,didConverge]=implicitWeakRungeKStep(y,t,a,B,deltaT,algorithm,aCur,BCur,useNewton,useGaussian,maxIter,RelTol,AbsTol)
%%IMPLICITWEAKRUNGEKSTEP Perform a single step of an implicit weak
%           stochastic Runge-Kutta method under It� calculus. This
%           integrates d-dimensional stochastic differential equation of
%           the form:
%           dy=a(y,t)*dt+B(y,t)*dW
%           where dW is the differential of an m-dimensional Wiener
%           process. As the stepsize used decreases, weak methods converge
%           such that integrals with the random process are a measure are
%           correct (for example, to determine moments). However, they do
%           not converge to the optimal path, unlike strong methods.
%
%INPUTS: y The dX1 initial value of the random process.
%        t The scalar initial time of the random process. If an empty
%          matrix is passed, t=0 is used.
%        a A function handle to the drift function. This is called as
%          a(y,t) and returns a dX1 vector. If one wishes to use Newton's
%          method for the implicit iteration, then the calling format is
%          [aVal,papy]=a(y,t), where papy is the dXd matrix of partial
%          derivatives of a with respect to the elements of y papy(:,i) is
%          the derivative with respect to the ith component of y.
%        B A function handle to the diffusion matrix function. This is
%          called as B(y,t) and returns a dXm matrix.
%   deltaT The time increment over which the step is taken.
% algorithm A parameter specifying the algorithm to use. Possible values
%          are:
%          0 Use the implicit order 2.0 weak scheme for scalar noise from
%            Equation 5.12 of Chapter 15.5 of [1], which is the same as
%            Equation 4.12 of Chapter 15.4 and is the implicit form of
%            Equation 1.1 in Chapter 15.1 of [1]. This requires that m=1.
%          1 Use the autonomous implicit order 2.0 weak scheme from
%            Equation 5.14 of Chapter 15.5 of [1], which is the same as
%            Equation 4.13 of Chapter 15.4 and is the implicit form of
%            Equation 1.3 in Chapter 15.1 of [1]. This requires that a and
%            B not depend on t.
%          2 Use the autonomous explicit order 2.0 weak scheme for additive
%            noise that comes as a special case of Equation 5.14 of Chapter
%            15.5 of [1]. This requires that a and B not depend on t and
%            that B does not depend on x.
% aCur, BCur Often one might already have the values a(x,t) and B(x,t). If
%          so, then they should be provided as the dX1 and dXm aCur and
%          BCur to avoid recalculation. If unavailable, these values can be
%          omitted or empty matrices passed.
% useNewton Indicates whether the implicit iteration should be performed
%          using Newton's method or fixed-point iteration. The default if
%          omitted or an empty matrix is passed is false (fixed-point
%          iteration). If Newton's method is used, then papy must be
%          returned by the function a.
% useGaussian Algorithms 0 and 1 have a choice of how the random component
%          is generated. If useGaussian=true, then Gaussian random
%          variables will be used. Otherwise, simpler random variables
%          having the same desired moment properties will be used. the
%          default if omitted or an empty matrix is passed is true.
%  maxIter The maximum number of iterations to perform. The default if
%          omitted or an empty matrix is passed is 2.
% RelTol, AbsTol The relative and absolute tolerances on the iterations
%          before declaring convergence. If these are set to 0 (the default
%          if omitted or empty matrices are passed), then the algorithm
%          will just iterate for the maximum number of iterations. the
%          tolerances apply to each element of x. Convergence is declared
%          if all(diff<=AbsTol)||all(diff<=RelTol*abs(yNext)).
%
%OUTPUTS: yNext The estimated value of the process after taking a step of
%               deltaT. This is a random value.
%   didConverge If RelTol and/or AbsTol are not zero and maxIter>0, then
%               this indicates whether the iterations converged to the
%               desired accuracy. Otherwise, this is just an empty matrix.
%
%EXAMPLE 1:
%This is an example of a nonlinear scalar problem with non-additive noise
%where an explicit solution is available as a basis of comparison. In
%Chapter 4.4 of [1], the stochastic differential equation and its solution
%are from Equation 4.40. We compare the performance of the explicit
%solution (taking the expected value using cubature integration) and the
%implicit solution.
% rng(1)%Make exact run repeatable.
% numMC=1e3;
% numSteps=1;
% algorithm=1;
% useNewton=true;
% deltaT=1.1;
% y0=0.5;
% aDrift=@(y,t)((1/3)*y^(1/3));
% BDiff=@(y,t)(y^(2/3));
% papy=@(y)1/(9*y^(2/3));
% aFun=@(y,t)dealRobust(aDrift(y,t),papy(y));
% explSim=@(W)(y0^(1/3)+(1/3)*W)^3;
%
% %Take the expected value of the explicit solution using quadrature
% %integration.
% [xi,w]=quadraturePoints1D(6);%2*6-1=11th order.
% numPts=length(w);
% xi=sqrt(deltaT)*xi;
% muCub=0;
% for k=1:numPts
%     muCub=muCub+w(k)*explSim(xi(:,k));
% end
%
% valsRKImp=zeros(1,numMC);
% valsRK=zeros(1,numMC);
% for curMC=1:numMC
%     y=y0;
%     yExp=y0;
%     t=0;
%     for curStep=1:numSteps
%         s=rng();%Record the state prior to generating the random variables.
%         y=implicitWeakRungeKStep(y,t,aFun,BDiff,deltaT/numSteps,algorithm,[],[],useNewton);
%
%         rng(s)%Drive the explicit step with the same random process as the
%               %implicit one.
%         yExp=weakRungeKStep(yExp,t,aDrift,BDiff,deltaT/numSteps,algorithm);
%         t=t+deltaT/numSteps;
%     end
%     valsRKImp(curMC)=y;
%     valsRK(curMC)=yExp;
% end
% muRKImp=mean(valsRKImp);
% muRK=mean(valsRK);
% abs((muRKImp-muCub)./muCub)%Relative mean error, implicit.
% abs((muRK-muCub)./muCub)%Relative mean error, explicit.
%The implicit error will be about 0.0086 and the explicit error will be
%about 0.0153. Thus, the algorithm improves the explicit method in this
%instance.
%
%EXAMPLE 2:
%Here, we compare the implicit strong Taylor method to the explicit method
%on a linear model. This is done with the same noise driving both
%processes.
% algorithm=2;
% useNewton=false;
% numMC=1e3;
% deltaT=1/3;
% numSteps=5;
% y0=[1/4;-12];
% A=[1.1,0.1;
%    -0.2,2.2];
% D=[1.5,-0.4;
%    0.1,1];
% d=size(D,1);
% [F,Q]=linDynMod2Disc(deltaT,A,D);
% mu=F*y0;
% P=Q;
% aDrift=@(x,t)(A*x);
% BDiff=@(x,t)(D);
%
% aFun=@(y,t)dealRobust(aDrift(y,t),A);
%
% valsRKImp=zeros(d,numMC);
% valsRK=zeros(d,numMC);
% for curMC=1:numMC
%     yExpl=y0;
%     y=y0;
%     t=0;
%     for curStep=1:numSteps
%         s=rng();%Record the state prior to generating the random variables.
%         y=implicitWeakRungeKStep(y,t,aFun,BDiff,deltaT/numSteps,algorithm,[],[],useNewton);
%
%         rng(s)%Drive the explicit step with the same random process as the
%               %implicit one.
%         yExpl=weakRungeKStep(yExpl,t,aDrift,BDiff,deltaT/numSteps,algorithm);
%         t=t+deltaT/numSteps;
%     end
%     valsRKImp(:,curMC)=y;
%     valsRK(:,curMC)=yExpl;
% end
% [muRKImp,PRKImp]=calcMixtureMoments(valsRKImp);
% [muRK,PRK]=calcMixtureMoments(valsRK);
% norm(muRKImp-mu)./norm(mu)%Relative mean error, implicit.
% norm(PRKImp-P,'fro')./norm(P,'fro')%Relative variance error,implicit.
% norm(muRK-mu)./norm(mu)%Relative mean error, explicit.
% norm(PRK-P,'fro')./norm(P,'fro')%Relative variance error,explicit.
%One will typically see that the relative errors of the implicit method are
%notably better than the explicit method.
%
%EXAMPLE 3:
%This is an example of what is considered a "stiff" problem in Section 12.2
%of [1] with non-additive noise. The problem has an explicit solution in
%terms of the Wiener process W. We use quadrature integration to obtain the
%mean of that solution, which is used as the true moments for comparison.
%We compare this implicit method with an explicit step. This is a 2X1
%process with scalar noise.
% rng(1)%Make the exact run repeatable.
% algorithm=0;
% useNewton=false;
% useGaussian=false;
% numMC=1e3;
% a=25;
% b=2;
% deltaT=1/10;
% numSteps=10;
% A=[-a,a;
%     a,-a];
% B=[b,0;
%    0,b];
% aDrift=@(y,t)A*y;
% BDiff=@(y,t)B*y;
% d=2;
% papy=A;
% aFun=@(y,t)dealRobust(aDrift(y,t),papy);
% %The explicit solution; equation 2.5 in Chapter 12.2 of [1].
% expSol=@(W,deltaT,y0)(expm((A-(1/2)*B^2)*deltaT+B*W)*y0);
% y0=[1;0.1];
%
% %Take the expected value of the explicit solution using quadrature
% %integration.
% [xi,w]=quadraturePoints1D(6);%2*6-1=11th order.
% numPts=length(w);
% xi=sqrt(deltaT)*xi;
% muCub=zeros(d,1);
% for k=1:numPts
%     muCub=muCub+w(k)*expSol(xi(:,k),deltaT,y0);
% end
%
% valsRK=zeros(d,numMC);
% valsRKExplicit=zeros(d,numMC);
% for curMC=1:numMC
%     y=y0;
%     yExp=y0;
%     t=0;
%     for curStep=1:numSteps
%         s=rng();%Record the state prior to generating the random variables.
%         y=implicitWeakRungeKStep(y,t,aFun,BDiff,deltaT/numSteps,algorithm,[],[],useNewton,useGaussian);
%
%         rng(s)%Drive the explicit step with the same random process as the
%               %implicit one.
%         yExp=weakRungeKStep(yExp,t,aDrift,BDiff,deltaT/numSteps,algorithm,[],[],useGaussian);
%         t=t+deltaT/numSteps;
%     end
%     valsRK(:,curMC)=y;
%     valsRKExplicit(:,curMC)=yExp;
% end
% muRK=mean(valsRK,2);
% muRKExp=mean(valsRKExplicit,2);
% norm((muRK-muCub)./norm(muCub))%Relative mean error, implicit.
% norm((muRKExp-muCub)./norm(muCub))%Relative mean error, explicit.
%One will get a relative implicit error of about 0.0119 and a relative
%explicit error of about 0.0121. Getting rid of the random seed, the
%implicit solution is typically more accurate than the explicit. Using the
%Gaussian noise option also improves performance.
%
%REFERENCES:
%[1] P. E. Kloeden and E. Platen, Numerical Solution of Stochastic
%    Differential Equations. Berlin: Springer, 1999.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<13||isempty(AbsTol))
    AbsTol=0;
end

if(nargin<12||isempty(RelTol))
    RelTol=0;
end

if(nargin<11||isempty(maxIter))
    maxIter=2;
end

if(nargin<10||isempty(useGaussian))
    useGaussian=true;
end

if(nargin<9||isempty(useNewton))
    useNewton=false;
end

if(nargin<8||isempty(BCur))
    BCur=B(y,t);
end

if(nargin<7||isempty(aCur))
    aCur=a(y,t);
end

if(isempty(t))
    t=0;
end

if(algorithm>=0&&algorithm<=2)
    [yNext,fixedTerm]=weakRungeKStep(y,t,a,B,deltaT,algorithm,aCur,BCur,useGaussian);

    tNext=t+deltaT;
    if(useNewton)
        [yNext,didConverge]=implicitNewtonIter(y,a,deltaT/2,yNext,tNext,fixedTerm,maxIter,RelTol,AbsTol);
    else
        [yNext,didConverge]=implicitFixedPointIter(y,a,deltaT/2,yNext,tNext,fixedTerm,maxIter,RelTol,AbsTol);
    end
else
    error('Unknown algorithm specified.')
end
end

function [yNext,didConverge]=implicitFixedPointIter(yOld,a,coeff,yNext,tNext,fixedTerm,maxIter,RelTol,AbsTol)
%IMPLICITFIXEDITER This iterates the function
%                  yNext=a(yNext,tNext)*coeff+fixedTerm
%                  Iterations continue until the RelTol and or AbsTol
%                  conditions are met or maxIter iterations have elapsed.
%
%December 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(RelTol==0&&AbsTol==0)
        %Fixed point iteration for the maximum number of steps.
        for curIter=1:maxIter
            yNext=a(yNext,tNext)*coeff+fixedTerm;
        end
        didConverge=[];
    else%Iterate until meeting the convergence bound.
        didConverge=false;
        for curIter=1:maxIter
            yNext=a(yNext,tNext)*coeff+fixedTerm;

            diff=abs(yOld-yNext);
            if(all(diff<=AbsTol)||all(diff<=RelTol*abs(yNext)))
                didConverge=true;
                break;
            end

            yOld=yNext;
        end
    end
end

function [yNext,didConverge]=implicitNewtonIter(yOld,a,coeff,yNext,tNext,fixedTerm,maxIter,RelTol,AbsTol)
%%IMPLICITNEWTONITER This function uses Newton's method to try to solve
%                    a(yNext,tNext)*coeff+fixedTerm-yNext=0
%                    for yNext.Iterations continue until the RelTol and or
%                    AbsTol conditions are met or maxIter iterations have
%                    elapsed.
%
%December 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    d=size(yNext,1);

    I=eye(d,d);
    if(RelTol==0&&AbsTol==0)
        %Newton's iteration for the maximum number of steps.
        for curIter=1:maxIter
            [aVal,papy]=a(yNext,tNext);
            F=yNext-fixedTerm-coeff*aVal;
            dF=I-coeff*papy;
            yNext=yNext-dF\F;
        end
        didConverge=[];
    else%Iterate until meeting the convergence bound.
        didConverge=false;
        for curIter=1:maxIter
            [aVal,papy]=a(yNext,tNext);
            F=aVal-fixedTerm-coeff*aVal;
            dF=I-coeff*papy;
            yNext=yNext-dF\F;

            diff=abs(yOld-yNext);
            if(all(diff<=AbsTol)||all(diff<=RelTol*abs(yNext)))
                didConverge=true;
                break;
            end

            yOld=yNext;
        end
    end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [theta_median theta_std theta_lbound theta_ubound sigma_median]=panel6estimates(d,N,n,T,theta_gibbs,sigma_gibbs,cband)

% obtain point estimates for the structural factors
% loop over sample periods
for ii=1:T
   % loop over structural factors
   for jj=1:d
   theta_median(jj,1,ii)=[quantile(theta_gibbs(jj,:,ii),0.5)];
   theta_std(jj,1,ii)=std(theta_gibbs(jj,:,ii));
   theta_lbound(jj,1,ii)=[quantile(theta_gibbs(jj,:,ii),(1-cband)/2)];
   theta_ubound(jj,1,ii)=[quantile(theta_gibbs(jj,:,ii),1-(1-cband)/2)];
   end
end

% obtain point estimates for sigma
% loop over sample periods
for ii=1:T
   % loop over sigma entries
   for jj=1:(N*n)^2
   sigma_median(jj,1,ii)=[quantile(sigma_gibbs(jj,:,ii),0.5)];
   end
end


function [Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch)
%MAKEYBUS   Builds the bus admittance matrix and branch admittance matrices.
%   [YBUS, YF, YT] = MAKEYBUS(MPC)
%   [YBUS, YF, YT] = MAKEYBUS(BASEMVA, BUS, BRANCH)
%
%   Returns the full bus admittance matrix (i.e. for all buses) and the
%   matrices YF and YT which, when multiplied by a complex voltage vector,
%   yield the vector currents injected into each line from the "from" and
%   "to" buses respectively of each line. Does appropriate conversions to p.u.
%   Inputs can be a MATPOWER case struct or individual BASEMVA, BUS and
%   BRANCH values. Bus numbers must be consecutive beginning at 1
%   (i.e. internal ordering).
%
%   See also MAKEJAC, MAKESBUS, EXT2INT.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   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.

%% extract from MPC if necessary
if nargin < 3
    mpc     = baseMVA;
    baseMVA = mpc.baseMVA;
    bus     = mpc.bus;
    branch  = mpc.branch;
end

%% constants
nb = size(bus, 1);          %% number of buses
nl = size(branch, 1);       %% number of lines

%% define named indices into bus, 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;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%% check that bus numbers are equal to indices to bus (one set of bus numbers)
if any(bus(:, BUS_I) ~= (1:nb)')
    error('makeYbus: buses must be numbered consecutively in bus matrix; use ext2int() to convert to internal ordering')
end

%% for each branch, compute the elements of the branch admittance matrix where
%%
%%      | If |   | Yff  Yft |   | Vf |
%%      |    | = |          | * |    |
%%      | It |   | Ytf  Ytt |   | Vt |
%%
stat = branch(:, BR_STATUS);                    %% ones at in-service branches
Ys = stat ./ (branch(:, BR_R) + 1j * branch(:, BR_X));  %% series admittance
Bc = stat .* branch(:, BR_B);                           %% line charging susceptance
tap = ones(nl, 1);                              %% default tap ratio = 1
i = find(branch(:, TAP));                       %% indices of non-zero tap ratios
tap(i) = branch(i, TAP);                        %% assign non-zero tap ratios
tap = tap .* exp(1j*pi/180 * branch(:, SHIFT)); %% add phase shifters
Ytt = Ys + 1j*Bc/2;
Yff = Ytt ./ (tap .* conj(tap));
Yft = - Ys ./ conj(tap);
Ytf = - Ys ./ tap;

%% compute shunt admittance
%% if Psh is the real power consumed by the shunt at V = 1.0 p.u.
%% and Qsh is the reactive power injected by the shunt at V = 1.0 p.u.
%% then Psh - j Qsh = V * conj(Ysh * V) = conj(Ysh) = Gs - j Bs,
%% i.e. Ysh = Psh + j Qsh, so ...
Ysh = (bus(:, GS) + 1j * bus(:, BS)) / baseMVA; %% vector of shunt admittances

%% bus indices
f = branch(:, F_BUS);                           %% list of "from" buses
t = branch(:, T_BUS);                           %% list of "to" buses

%% for best performance, choose method based on MATLAB vs Octave and size
if nb < 300 || have_feature('octave')   %% small case OR running on Octave
    %% build Yf and Yt such that Yf * V is the vector of complex branch currents injected
    %% at each branch's "from" bus, and Yt is the same for the "to" bus end
    i = [1:nl 1:nl]';                           %% double set of row indices
    Yf = sparse(i, [f; t], [Yff; Yft], nl, nb);
    Yt = sparse(i, [f; t], [Ytf; Ytt], nl, nb);

    %% build Ybus
    Ybus = sparse([f;f;t;t], [f;t;f;t], [Yff;Yft;Ytf;Ytt], nb, nb) + ... %% branch admittances
            sparse(1:nb, 1:nb, Ysh, nb, nb);        %% shunt admittance
else                                %% large case running on MATLAB
    %% build connection matrices
    Cf = sparse(1:nl, f, ones(nl, 1), nl, nb);      %% connection matrix for line & from buses
    Ct = sparse(1:nl, t, ones(nl, 1), nl, nb);      %% connection matrix for line & to buses

    %% build Yf and Yt such that Yf * V is the vector of complex branch currents injected
    %% at each branch's "from" bus, and Yt is the same for the "to" bus end
    Yf = sparse(1:nl, 1:nl, Yff, nl, nl) * Cf + sparse(1:nl, 1:nl, Yft, nl, nl) * Ct;
    Yt = sparse(1:nl, 1:nl, Ytf, nl, nl) * Cf + sparse(1:nl, 1:nl, Ytt, nl, nl) * Ct;

    %% build Ybus
    Ybus = Cf' * Yf + Ct' * Yt + ...            %% branch admittances
            sparse(1:nb, 1:nb, Ysh, nb, nb);    %% shunt admittance
end


% DEMSPGP1DGP5 Do a simple 1-D regression after Snelson & Ghahramani's example.

% GP

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

dataSetName = 'spgp1d';
experimentNo = 5;

% load data
[X, y] = mapLoadData(dataSetName);

% Set up model
options = gpOptions('dtcvar');
options.numActive = 9;
options.optimiser = 'scg';

% use the deterministic training conditional.
q = size(X, 2);
d = size(y, 2);

model = gpCreate(q, d, X, y, options);
model.X_u = randn(9, 1)*0.25 - 0.75;
params = gpExtractParam(model);
model = gpExpandParam(model, params);

% Optimise the model.
iters = 1000;
display = 1;

model = gpOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

demSpgp1dPlot


function [Z]=rplus(X,Y)
%
% Replicating addition
%
% Does element by element operations on X and Y where non-same sized
% dimensions are implicity wrapped round to match the size of the larger
% to give a result matrix Z with size max(size(X),size(Y));
%
% In this case returns double array with X+Y
%
% See also repops, plus, minus
%
% Copyright 2006- by Jason D.R. Farquhar (jdrf@zepler.org)
% Inspired by code by Douglas M. Schwarz & Aki Vehtari.
Z=repop(X,'+',Y);


function calpak_test26 ( )

%*****************************************************************************80
%
%% CALPAK_TEST26 tests JED_TO_YMDF_SAKA and YMDF_TO_JED_SAKA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST26\n' );
  fprintf ( 1, '  For the Saka calendar:\n' );
  fprintf ( 1, '  JED_TO_YMDF_SAKA: JED -> YMDF.\n' );
  fprintf ( 1, '  YMDF_TO_JED_SAKA: YMDF -> JED.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  JED (in)    YMDF                JED (out)\n' );
  fprintf ( 1, '\n' );

  jed_epoch = epoch_to_jed_saka ( );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed1 = jed_test ( i );

    if ( jed1 < 0.0 )
      break
    end

    if ( jed_epoch <= jed1 )

      [ y2, m2, d2, f2 ] = jed_to_ymdf_saka ( jed1 );

      s2 = ymdf_to_s_numeric ( y2, m2, d2, f2 );

      jed3 = ymdf_to_jed_saka ( y2, m2, d2, f2 );

      fprintf ( 1, '  %11.2f  %20s  %11.2f\n', jed1, s2, jed3 );

    end

  end

  return
end


function pass = test_laplacian(pref)
% Test LAPLACIAN

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 50*pref.cheb3Prefs.chebfun3eps;

% Check definition:
F = chebfun3(@(x,y,z) cos(x.*z)+sin(y.*z));
lapF = laplacian(F);
lapF1 = diff(F, 2, 1) + diff(F, 2, 2) + diff(F, 2, 3);
pass(1) = norm(lapF1 - lapF) < tol;

% Check definition:
F = chebfun3(@(x,y,z) cos(x) + y.*z + z.^2);
lapF = laplacian(F);
lapF1 = diff(F, 2, 1) + diff(F, 2, 2) + diff(F, 2, 3);
pass(2) = norm(lapF1 - lapF) < tol;

end


close all; clear all;

%% Setting of the problem
global s
pde = fracLapdata8;
pde.L = 1;
option.theta = 0.3;
option.estType = 'star';
option.maxIt = 17;
option.maxN = 5e4;
option.solver = 'mg';
option.tol = 1e-6;
[node,elem] = squaremesh([-1,1,-1,1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');

% %% s = 0.2
% s = 0.2;
% afemfracLap(node,elem,pde,bdFlag,option);
%
% %% s = 0.4
% s = 0.4;
% afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
afemfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
afemfracLap(node,elem,pde,bdFlag,option);


function [targetMissDist, dVNorm, hOrbit, dVVect, dVVectNTW, eRVect, xferOrbit, departUT, timeSOI] = departureTargetMissDistance(x, dVVectNTW, eOrbit, departBodyInfo, arriveBodyInfo, parentBodyInfo, departUT, arrivalUT)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
    eTru = x(1);
    progradeDV = x(2);
    dVVectNTW(1) = progradeDV;

    eSMA = eOrbit(1);
    eEcc = eOrbit(2);
    eInc = eOrbit(3);
    eRAAN = eOrbit(4);
    eArg = eOrbit(5);
%     eTru = eOrbit(6);

    eTruAtDepartUT = eOrbit(6);
    eMeanMotion = computeMeanMotion(eSMA, departBodyInfo.gm);

    eMean = computeMeanFromTrueAnom(eTru, eEcc);
    eMeanAtDepartUT = computeMeanFromTrueAnom(eTruAtDepartUT, eEcc);
    departDeltaTBurnPosAdjust = (eMean-eMeanAtDepartUT)/eMeanMotion;
    departUT = departUT + departDeltaTBurnPosAdjust;

    [eRVect,eVvect] = getStatefromKepler(eSMA, eEcc, eInc, eRAAN, eArg, eTru, departBodyInfo.gm);
    tHat = eVvect/norm(eVvect);
    wHat = cross(eRVect,eVvect)/norm(cross(eRVect,eVvect));
    nHat = cross(tHat,wHat)/norm(cross(tHat,wHat));
    ECI2TWNRotMat = [tHat,wHat,nHat];
    dVVect = ECI2TWNRotMat * dVVectNTW;
    dVNorm = norm(dVVect);

    hRVect = eRVect;
    hVVect = eVvect + dVVect;
    [hSMA, hECC, hINC, hRAAN, hARG, hTRU] = getKeplerFromState(hRVect,hVVect,departBodyInfo.gm);
    hOrbit = [hSMA, hECC, hINC, hRAAN, hARG, hTRU];

    rSOI = getSOIRadius(departBodyInfo, parentBodyInfo);

    hTruSOI = computeTrueAFromRadiusEcc(rSOI, hOrbit(1), hOrbit(2));
    [rVectSOI,vVectSOI]=getStatefromKepler(hOrbit(1), hOrbit(2), hOrbit(3), hOrbit(4), hOrbit(5), hTruSOI, departBodyInfo.gm);
    hOrbit(7) = hTruSOI;

    meanDepartTime = computeMeanFromTrueAnom(hOrbit(6), hOrbit(2));
    meanSOITime = computeMeanFromTrueAnom(hTruSOI, hOrbit(2));
    hMeanMotion = computeMeanMotion(hOrbit(1), departBodyInfo.gm);
    deltaT2SOI = (meanSOITime-meanDepartTime)/hMeanMotion;

    timeSOI = departUT - deltaT2SOI; %may be a + sign here
    [departBodyRVect, departBodyVVect] = getStateAtTime(departBodyInfo, timeSOI, parentBodyInfo.gm);
    scInParentOrbitRVect = departBodyRVect + rVectSOI;
    scInParentOrbitVVect = departBodyVVect + vVectSOI;
    [smaXAct, eccXAct, incXAct, raanXAct, argXAct, truDXAct] = getKeplerFromState(scInParentOrbitRVect,scInParentOrbitVVect,parentBodyInfo.gm);
    xferOrbit = [smaXAct, eccXAct, incXAct, raanXAct, argXAct, truDXAct];

    bodyInfoXAct.sma = smaXAct;
    bodyInfoXAct.ecc = eccXAct;
    bodyInfoXAct.inc = rad2deg(incXAct);
    bodyInfoXAct.raan = rad2deg(raanXAct);
    bodyInfoXAct.arg = rad2deg(argXAct);
    bodyInfoXAct.mean = computeMeanFromTrueAnom(truDXAct, eccXAct);
    bodyInfoXAct.epoch = timeSOI;

    [rVectXAct, vVectXAct] = getStateAtTime(bodyInfoXAct, arrivalUT, parentBodyInfo.gm);
    [~, ~, ~, ~, ~, truAXAct] = getKeplerFromState(rVectXAct,vVectXAct,parentBodyInfo.gm);
    xferOrbit(7) = truAXAct;
    [rVectArriveBody, ~] = getStateAtTime(arriveBodyInfo, arrivalUT, parentBodyInfo.gm);
    targetMissDist = norm(rVectArriveBody-rVectXAct);
end


function f1 = p06_f1 ( x )

%*****************************************************************************80
%
%% P06_F1 evaluates the first derivative for problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the value of the variable.
%
%    Output, real F1, the first derivative of the
%    objective function.
%
  f1 = -1.0;

  return
end


function intrec(Z)
%INTREC Performs morphological character recognition
%INTREC(X) recognizes the characters (in integer form) present in the
%image X and displays it as output. INTREC uses morphological operations(like
%Dilation and Hit-or-Miss transform) to recognize characters.The charecter
%size of the integers present in the image should be exactly 26, otherwise
%it may not recognize it.
%
%Example:
%X=imread('testimage1.bmp');
%imshow(X)
%intrec(X)
%The digits found in the image are:
%0
%3
%5
%--------------------------------------------------------------------------
%Authors: Jahanzeb Rajput and Mohammad Fahad
%Department of Electrical Engineering
%University of Engineering and Technology, Lahore, Pakistan.
%--------------------------------------------------------------------------

A= imread ('zero.bmp');
B= imread ('one.bmp');
C= imread ('two.bmp');
D= imread ('three.bmp');
E= imread ('four.bmp');
F= imread ('five.bmp');
G= imread ('six.bmp');
H= imread ('seven.bmp');
I= imread ('eight.bmp');
J= imread ('nine.bmp');
%SE 2
SE = strel('square',3);

K=imdilate(A,SE);A2=K-A;

L=imdilate(B,SE);B2=L-B;

M=imdilate(C,SE);C2=M-C;

N=imdilate(D,SE);D2=N-D;

O=imdilate(E,SE);E2=O-E;

P=imdilate(F,SE);F2=P-F;

Q=imdilate(G,SE);G2=Q-G;

R=imdilate(H,SE);H2=R-H;

S=imdilate(I,SE);I2=S-I;

T=imdilate(J,SE);J2=T-J;
%-------------------------
%Hit or Miss
%-------------------------
disp('The digits found in the image are:');
if ~isempty(nonzeros(bwhitmiss(Z,A,A2)))
  disp('0');
end
%imshow('num1.bmp')
%U=bwhitmiss(Z,F,F2);
%figure
%imshow(U)
if ~isempty(nonzeros(bwhitmiss(Z,B,B2)))
    disp('1');
end

if ~isempty(nonzeros(bwhitmiss(Z,C,C2)))
   disp('2');
end

if ~isempty(nonzeros(bwhitmiss(Z,D,D2)))
    disp('3');
end

if ~isempty(nonzeros(bwhitmiss(Z,E,E2)))
   disp('4');
end

if ~isempty(nonzeros(bwhitmiss(Z,F,F2)))
   disp('5');
end

if ~isempty(nonzeros(bwhitmiss(Z,G,G2)))
   disp('6');
end

if ~isempty(nonzeros(bwhitmiss(Z,H,H2)))
   disp('7');
end

if ~isempty(nonzeros(bwhitmiss(Z,I,I2)))
   disp('8');
end

if ~isempty(nonzeros(bwhitmiss(Z,J,J2)))
   disp('9');
end


%function []=mcmc()

clear all; clc;

%% MCMC options
nloop=110; % number of iterations for mcmcm
burnin=10;  % number of initial iterations to discard
thin=10;    % save every nth iteration
M=4;        %number of group components desired

%% set starting values for Z, S_BAR, SIGMA_S B_BAR SIGMA_B MU
load real_data;
[Z_st S_BAR_st SIGMA_S_st B_BAR_st SIGMA_B_st N_k_st N_k1k2_st]=get_initial_values_new(N,M_i,S,B,Q,M);
Z=Z_st; % Mi x M matrices of group indicators
S_BAR=S_BAR_st; % cluster centroid locations
SIGMA_S=SIGMA_S_st; % cluster centroid variances
B_BAR=B_BAR_st; % group level connectivities
SIGMA_B=SIGMA_B_st; % variances of connectivities
N_k=N_k_st; % number of components that belong to each cluster
N_k1k2=N_k1k2_st; % pairwise counts of group level clusters
MU=ones(M,1)/M; %probabilities of membership in group-level clusters.

%% initialize arrays of posterior draws
keep=round((nloop-burnin)/thin);
Z_array=cell(keep,1);
S_BAR_array=cell(keep,1);
SIGMA_S_array=cell(keep,1);
B_BAR_array=cell(keep,1);
SIGMA_B_array=cell(keep,1);
MU_array=cell(keep,1);
iter_array=0;

%%Hyperparameters
c=10000;                    %'c' in the paper
eta=median(N_k_st);         % 'eta' in paper
SS=eta*mean(SIGMA_S,3);     % 'S' in the paper
p1=1;                       % 'a' in the paper
p2=1;                       % 'b' in the paper

%% run MCMC

for iter=1:nloop

    % Draw S_BAR (group centroid locations)
    for k=1:M
        mu_s_bar_k=zeros(3,1);
        Sigma_s_bar_k=inv(N_k(k)*inv(SIGMA_S(:,:,k))+inv(SS));
        for i=1:N
            if max(Z{i}(:,k))==1
                j_k=find(Z{i}(:,k)==1);
                for j=1:length(j_k)
                    mu_s_bar_k=mu_s_bar_k+S{i}(j_k(j),:)';
                end
            end
        end
        mu_s_bar_k=Sigma_s_bar_k*inv(SIGMA_S(:,:,k))*mu_s_bar_k;
        S_BAR(k,:)=mvnrnd(mu_s_bar_k,Sigma_s_bar_k);
    end

    % Draw SIGMA_S (group centoid covariance matrices)
    for k=1:M
        eta_k=eta+.5*N_k(k);
        SS_k=SS;
        for i=1:N
            if max(Z{i}(:,k))==1
                j_k=find(Z{i}(:,k)==1);
                for j=1:length(j_k)
                    SS_k=SS_k+.5*(S{i}(j_k(j),:)-S_BAR(k,:))'*(S{i}(j_k(j),:)-S_BAR(k,:));
                end
            end
        end
        SS_k=.5*(SS_k+SS_k');
        inv_SS_k=inv(SS_k);
        inv_SS_k=.5*(inv_SS_k+inv_SS_k');
        SIGMA_S(:,:,k)=inv(wishrnd(inv_SS_k,eta_k));
    end

    % Draw B_BAR (group mean connectivites)
    for k1=1:M
        for k2=1:M
            mu_b_bar_k1k2=zeros(Q,1);
            Sigma_sq_b_k1k2=1/(1/c+N_k1k2(k1,k2)/SIGMA_B(k1,k2))*eye(Q);
            for i=1:N
                n_k1=sum(Z{i}(:,k1));
                n_k2=sum(Z{i}(:,k2));
                if(and(n_k1>=1,n_k2>=1))
                    j_k1=find(Z{i}(:,k1)==1);
                    j_k2=find(Z{i}(:,k2)==1);
                    for j1=1:n_k1
                        for j2=1:n_k2
                            mu_b_bar_k1k2=mu_b_bar_k1k2+B{i}{j_k1(j1),j_k2(j2)}';
                        end
                    end
                end
            end
            mu_b_bar_k1k2=Sigma_sq_b_k1k2*mu_b_bar_k1k2/SIGMA_B(k1,k2);
            B_BAR{k1,k2}=mvnrnd(mu_b_bar_k1k2,Sigma_sq_b_k1k2);
        end
    end

    % Draw SIGMA_B (group connectivity variances)
    for k1=1:M
        for k2=1:M
            p1_k1k2=p1+.5*Q*N_k1k2(k1,k2);
            p2_k1k2=p2;
            for i=1:N
                n_k1=sum(Z{i}(:,k1));
                n_k2=sum(Z{i}(:,k2));
                if(and(n_k1>=1,n_k2>=1))
                    j_k1=find(Z{i}(:,k1)==1);
                    j_k2=find(Z{i}(:,k2)==1);
                    for j1=1:n_k1
                        for j2=1:n_k2
                            p2_k1k2=p2_k1k2+.5*sum((B{i}{j_k1(j1),j_k2(j2)}-B_BAR{k1,k2}).^2);
                        end
                    end
                end
            end
            SIGMA_B(k1,k2)=1/gamrnd(p1_k1k2,1/p2_k1k2);
        end
    end

    % Draw Z (indicators of group membership)
    ind=randperm(N);
    for i=ind
        ind_i=randperm(M_i(i));
        for j=ind_i
            Log_p_ij=zeros(M,1);
            for k=1:M
                Sigma_s_ijk=SIGMA_S(:,:,k);
                s_ijk=S{i}(j,:)-S_BAR(k,:);
                log_p_s_ijk=-.5*log(det(Sigma_s_ijk))-.5*s_ijk*inv(Sigma_s_ijk)*s_ijk';
                log_p_b_ijk=0;
                for j1=1:M_i(i)
                    if j==j1
                        log_p_b_ijk=log_p_b_ijk-.5*Q*log(det(SIGMA_B(k,k)))...
                        -.5*sum((B{i}{j,j}-B_BAR{k,k}).^2)/SIGMA_B(k,k);
                    end
                    if and(not(j==j1),find(Z{i}(j1,:)==1)==k)
                        log_p_b_ijk=log_p_b_ijk-.5*Q*log(det(SIGMA_B(k,k)))...
                        -.5*sum((B{i}{j1,j1}-B_BAR{find(Z{i}(j1,:)==1),find(Z{i}(j1,:)==1)}).^2)/...
                            SIGMA_B(find(Z{i}(j,:)==1),find(Z{i}(j,:)==1));
                    end
                    if not(find(Z{i}(j,:)==1)==find(Z{i}(j1,:)==1))
                        log_p_b_ijk=log_p_b_ijk-.5*Q*log(det(SIGMA_B(k,find(Z{i}(j1,:)==1))))...
                        -.5*sum((B{i}{j,j1}-B_BAR{k,find(Z{i}(j1,:)==1)}).^2)/...
                              SIGMA_B(k,find(Z{i}(j1,:)==1));
                    end
                end
                for j2=1:M_i(i)
                    if not(find(Z{i}(j2,:)==1)==find(Z{i}(j,:)==1))
                        log_p_b_ijk=log_p_b_ijk-.5*Q*log(det(SIGMA_B(find(Z{i}(j2,:)==1),k)))...
                        -.5*sum((B{i}{j2,j}-B_BAR{find(Z{i}(j2,:)==1),k}).^2)/...
                                SIGMA_B(find(Z{i}(j2,:)==1),k);
                    end
                end
                Log_p_ij(k)=MU(k)+log_p_s_ijk+log_p_b_ijk;
            end
            Log_p_ij=Log_p_ij-max(Log_p_ij);
            P_ij=exp(Log_p_ij)/sum(exp(Log_p_ij));
            v=rand;
            Z{i}(j,:)=0*Z{i}(j,:);
            if v<=P_ij(1)
                   Z{i}(j,1)=1;
                   log_p_ij=Log_p_ij(1);
            end
            for k=1:(M-1)
               if(and(v>sum(P_ij(1:k)),v<=sum(P_ij(1:(k+1)))))
                   Z{i}(j,k+1)=1;
                   log_p_ij=Log_p_ij(k);
               end
            end
        end
    end

    % Draw MU (probabilities of clusters)
    N_k1k2=zeros(M);
    for k1=1:M
        for k2=1:M
            for i=1:N
                n_k1=sum(Z{i}(:,k1));
                n_k2=sum(Z{i}(:,k2));
                if(and(n_k1>=1,n_k2>=1))
                    if not(k1==k2)
                        N_k1k2(k1,k2)=N_k1k2(k1,k2)+n_k1*n_k2;
                    end
                    if k1==k2
                        N_k1k2(k1,k2)=N_k1k2(k1,k2)+n_k1;
                    end
                end
            end
        end
    end
    N_k=diag(N_k1k2);
    MU=drchrnd(1/M*ones(1,M)+N_k',1);

    % Posterior Draw arrays
    if and(iter>=burnin,thin*round(iter/thin)==iter)
        iter_array=iter_array+1;
        Z_array{iter_array}=Z;
        S_BAR_array{iter_array}=S_BAR;
        SIGMA_S_array{iter_array}=SIGMA_S;
        B_BAR_array{iter_array}=B_BAR;
        SIGMA_B_array{iter_array}=SIGMA_B;
        MU_array{iter_array}=MU;

        %compute and print out intermediate estimates of source locations
        S_BAR_mean=S_BAR_array{1};
        for j=2:iter_array
            S_BAR_mean=S_BAR_mean+S_BAR_array{j};
        end
        S_BAR_mean=S_BAR_mean/iter_array;
        iter
        [S_BAR_st zeros(M,1) S_BAR_mean]

        %[sqrt(sum(min(pdist2(S_BAR_true,S_BAR_st)').^2))/M sqrt(sum(min(pdist2(S_BAR_true,S_BAR_mean)').^2))/M]

        %compute and print intermediate indicator probs for random subject
        i=discretesample(repmat(1,N,1)/N,1);
        Z_i_mean=Z_array{1}{i};
        for j=2:iter_array
           Z_i_mean=Z_i_mean+Z_array{j}{i};
        end
        Z_st{i}
        Z_i_mean=Z_i_mean/iter_array
    end

end

save mcmc_outputs nloop M Z_array S_BAR_array SIGMA_S_array B_BAR_array SIGMA_B_array MU_array iter_array


%rf_combineRF.m
%Peter Truong, Sunnybrook Research Institute 2021.
%
% USAGE:
% [rf,AMPINT]=rf_combineRF(rf_struct1,rf_struct2);
%
% DESCRIPTION:
% Combines two similar rf waveforms (e.g. same pulse, but one frequency
% shifted), in FID-A rf pulse structure format.
%
% INPUTS:
% rf_struct1 = RF pulse definition structure.
% rf_struct2 = RF pulse definition structure, similar to rf_struct1

%
% OUTPUTS:
% rf         = Output rf waveform of the combined rf pulse, in FID-A rf
%              pulse structure format.
% AMPINT     = Calculated amplitude integral (for use in Siemens .pta files).
function [rf,AMPINT]=rf_combineRF(rf_struct1,rf_struct2)

%Convert waveforms to complex form
rf1_waveform=rf_struct1.waveform(:,2).*exp(1i.*rf_struct1.waveform(:,1)*pi/180);
rf2_waveform=rf_struct2.waveform(:,2).*exp(1i.*rf_struct2.waveform(:,1)*pi/180);

%Here we compute the Amplitude integral(AMPINT), which is used by magnetom
%to calculate the transmitter power that is required in order to achieve
%the desired flip angle
rf1_waveform_scaled=rf1_waveform./max(abs(rf1_waveform));
AI=sum(rf1_waveform_scaled);

combined_waveform=rf1_waveform + rf2_waveform;
combined_waveform_scaled=combined_waveform./max(abs(combined_waveform));
AMPINT=AI./max(abs(combined_waveform_scaled));

rf=rf_struct1;
rf.waveform(:,1)=phase(combined_waveform_scaled).*180/pi;
rf.waveform(:,2)=abs(combined_waveform_scaled);

% updating time-bandwidth product & time-w1max product
rf.tbw=rf_struct1.tbw+rf_struct2.tbw;
rf.tw1=rf_struct1.tw1+rf_struct2.tw1;


function [R,theta] = round_sra(X,opt)
%% Given the moment matrix X, round R and theta from X
if nargin < 2
    opt = 1;
end
X     = X{1};
n     = size(X,1);
N     = round((n-10)/10);
[V,D] = eig(X);
[~,I] = sort(diag(D),'descend');
V     = V(:,I);
%% take the opt-th eigenvector
nropt = length(opt);
x     = V(:,opt);
R     = zeros(3,3,nropt);
theta = zeros(N,nropt);
for k = 1:nropt
    xk    = x(:,k);
    xk    = xk/xk(1);
    %% round that eigenvector
    Rk    = reshape( xk(1+blkIndices(1,9)),3,3 );
    Rk    = project2SO3(Rk);

    thetak= sign( xk(10+1:10+N) );
    thetak(thetak==0) = 1;

    R(:,:,k)     = Rk;
    theta(:,k)   = thetak;
end
end


%% FUNCTION Least_Weight2FGLasso
%   Weighted Fused Group Lasso and Weighted Lasso with Least Squares Loss .
%
%% OBJECTIVE
%   argmin_W { \sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%              + \sum_{i=1}^d rho1{i} * \|W(i, :)\|_1
%              + \sum_{i=1}^d rho2{i} \sum_{j=1}^{t-1}* \|W(i, j) - W(i, j+1)\|_1
%          }
%   rho1: weighted Lasso sparse.
%   rho2: weighted Fused Lasso.
%   R encodes fused structure relationship [1 -1 0 ...; 0 1 -1 ...; ...]
%      R=zeros(t,t-1);R(1:(t+1):end)=1;R(2:(t+1):end)=-1;
%
%% INPUT
% X: {d * n} * t - input matrix
% Y: {n * 1} * t - output matrix
% rho1: d * 1    - weighted Lasso.
% rho2: d * 1    - weighted fused Lasso.
%
%% OUTPUT
% W: model: d * t
% funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Jun Liu and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Zhou, J., Jun, L., Narayan, A. V. and Ye, J.  Modeling Disease
%   Progression via Fused Sparse Group Lasso. KDD 2012
%
%% RELATED FUNCTIONS
%   Least_NCFGLasso, init_opts

function [W, funcVal] = Least_Weight2FGLasso(X, Y, rho1, rho2, opts)

if nargin <5
    opts = [];
end

% initialize options.
opts=init_opts(opts);

task_num  = length (X);
dimension = size(X{1}, 1);

if length(rho1) ~= dimension
    error('Size of rho1 is not correct! Should be equivalent to dimension')
end

if length(rho2) ~= dimension
    error('Size of rho2 is not correct! Should be equivalent to dimension')
end

funcVal = [];

% Relation
R=zeros(task_num,task_num-1);
R(1:(task_num+1):end)=1;
R(2:(task_num+1):end)=-1;
R = R';

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = W0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0=W0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws);
    Fs   = funVal_eval (Ws);

    while true
        Wzp = ReweightFGLasso_projection(Ws - gWs/gamma, rho1 ./ gamma, rho2 ./ gamma);
        Fzp = funVal_eval  (Wzp);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        %         Fzp_gamma = Fs + trace(delta_Wzp' * gWs)...
        %             + gamma/2 * norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + sum(sum(delta_Wzp.* gWs))...
            + gamma/2 * r_sum;

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    funcVal = cat(1, funcVal, Fzp + nonsmooth_eval(Wz, rho1, rho2));

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions

    function [Wp] = ReweightFGLasso_projection (W, lambda_1, lambda_2)
        % solve it in row wise, since that
        % \sum_i^d rho1{i} * \|W(i, :)\|_1 is row coupled.
        % for each row we need to solve the proximal opterator
        % W(i, :) = argmin_w { 0.5 \|w - v\|_2^2
        %            + lambda_1{i} * \|w\|_1
        %            + lambda_2{i} * \|R * w\|_1 }
        % NOTE: Here the R is t-1 * t, the outside R is t * t-1, and
        Wp = zeros(size(W));

        for i = 1 : size(W, 1)
            v = W(i, :);

            w0 = zeros(length(v)-1, 1);
            w = flsa(v, w0,  lambda_1(i), lambda_2(i), length(v), 1000, 1e-9, 1, 6);

            Wp(i, :) = w';
        end
    end

% smooth part gradient.
    function [grad_W] = gradVal_eval(W)
        if opts.pFlag
            grad_W = zeros(size(W));
            parfor i = 1:task_num
                grad_W(:, i) = X{i}*(X{i}' * W(:,i)-Y{i});
            end
        else
            grad_W = [];
            for i = 1:task_num
                grad_W = cat(2, grad_W, X{i}*(X{i}' * W(:,i)-Y{i}) );
            end
        end
    end

% smooth part gradient.
    function [funcVal] = funVal_eval (W)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
    end

    function [non_smooth_value] = nonsmooth_eval(W, rho_1, rho_2)
        non_smooth_value = 0;
        for i = 1 : size(W, 1)
            w = W(i, :);
            non_smooth_value = non_smooth_value ...
                + rho_1(i) * norm(w, 1) + rho_2(i) * norm(R * w', 1);
        end
    end

end


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


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR:  rotating an US image
%
%   - load US data
%   - set image viewer and interpolator and trafo=rigid2D
%   - run a loop over different angles
%==============================================================================

clear, close all, help(mfilename)

%% load data
Tdata = double(imread('US.jpg'));
omega = [0,size(Tdata,1),0,size(Tdata,2)];
m     = [192,128] ;
xc    = getCellCenteredGrid(omega,m);

% setup interpolation scheme and image viewer
imgModel('reset','imgModel','linearInter');
viewImage('reset','viewImage','viewImage2D','colormap',gray(256),'axis','off');
fprintf('%20s : %s\n','viewImage',viewImage);
fprintf('%20s : %s\n','image model',imgModel);

% shortcuts for plotting stuff
Grid   = @(X)   plotGrid(X,omega,m,'spacing',8,'color','w');
dimstr = @(m)   sprintf('%s=[%s]',inputname(1),sprintf(' %d',m));
Title  = @(s,t) title([s,sprintf(', t=%s',num2str(t))],'fontsize',30);

%% display initial image
FAIRfigure(1,'figname',mfilename); clf; subplot(1,2,1);
viewImage(imgModel(Tdata,omega,xc),omega,m); hold on; gh = Grid(xc);
title(sprintf('%s, %s','data',dimstr(m)),'fontsize',30);

%% initialize transformation
trafo('set','trafo','rigid2D');
S = @(t) t/6;

% y(t) = R(t)*x+(I-R(t))*c,
%
% R(t) = [cos(t) -sin(t) ], c = [omega(1)]/2
%        [sin(w)  cos(t) ]      [omega(2)]
%
% or y(t) = rigid2D(w,x), where w = [t;(I-R(t))omega'/2].
c  = (omega(2:2:end)+omega(1:2:end))'/2
wc = @(t) [t;(eye(2)-[ cos(t),-sin(t);sin(t),cos(t)])*c];

%=======================================================================================
% the loop over time t
%=======================================================================================
t = 0.35*pi*sin(linspace(0,2.5*pi,101));

for j=1:length(t),
  yc = trafo(wc(t(j)),xc);      % compute the transformed points
  Tc = imgModel(Tdata,omega,yc);   % compute the transformed image

  if j == 1,              % initialize visualization
    figure(1); subplot(1,2,1); %colordef(gcf,'black');
    set(gh,'visible','off'); gh = Grid(yc);
    title(sprintf('%s, %s','data',dimstr(m)),'fontsize',30);
    subplot(1,2,2); vh = viewImage(Tc,omega,m); Title(trafo,t(j));
    FAIRpause;
  else,                   % continue plots
    subplot(1,2,1); set(gh,'visible','off');        gh = Grid(yc);
    axis(omega)
    subplot(1,2,2); set(vh,'cdata',reshape(Tc,m)'); Title(trafo,t(j));
    FAIRpause(1/2000)
  end;
  drawnow; fprintf('.'); if rem(j,50) == 0, fprintf('\n'); end;
end;
fprintf('\n')
%=======================================================================================


function [ correlations, rms, mean_correlation, mean_RMSE, long_correlation, long_RMSE, predictions, gts ] = evaluate_CCNF_model( alphas, betas, thetas, x, y, similarityFNs, sparsityFNs, offset, scaling, verbose, PrecalcQ2sFlat)
%evaluate_CCNF_model Evaluate the trained model on test (or training data)

% For visualising time series predictions
num_x_plots = 8;
num_y_plots = 10;

total_plots = num_x_plots * num_y_plots;

if(iscell(x))
    num_seqs = numel(x);
    x = cell2mat(x)';
    % add a bias term
    x =  cat(1, ones(1,size(x,2)), x);
else
    % if not a cell it has already been flattened, and is constant
    % (most likely)
    num_seqs = size(y,2);

end

% if not sure about const assume it is not
const = false;

if(nargin < 11)
    [ ~, ~, PrecalcQ2sFlat, ~ ] = CalculateSimilarities( num_seqs, x, similarityFNs, sparsityFNs, y, const);
end

correlations = zeros(num_seqs, 1);
rms = zeros(num_seqs, 1);

% concatenated data for an alternative correlation
y_predConcat = [];
y_trueConcat = [];

% Predict each sequence
for q=1:num_seqs

    if(iscell(y))
        seq_length = size(y{q},1);
        yq = y{q};
    else
        seq_length = size(y,1);
        yq = y(:,q);
    end

    X = x(:,(q-1)*seq_length+1:q*seq_length);

    h1 = 1./(1 + exp(-thetas * X));
    b = (2 * alphas' * h1)';

    PrecalcQ2flat = PrecalcQ2sFlat{q};

    precalc_eye = eye(seq_length);
    precalc_zeros = zeros(seq_length);

    SigmaInv = CalcSigmaCCNFflat(alphas, betas, seq_length, PrecalcQ2flat, precalc_eye, precalc_zeros);

    y_est = SigmaInv \ b;

    % Can optionally supply the scaling and offset used on the training
    % labels to be applied inversely
    y_est = y_est/scaling + offset;

    if(numel(y_est) > 1)
        R = corrcoef(y_est, yq);
        correlations(q) = R(1,2);
    end

    rms(q) = sqrt( mean((y_est - yq).^2) );

    y_predConcat = cat(1, y_predConcat, y_est);
    y_trueConcat = cat(1, y_trueConcat, yq);

    if(verbose)

        if(mod(q,total_plots) == 1)
            figure;
            remainingPlots = nExamples - q;
            if(remainingPlots < total_plots)
                num_y_plots = ceil(remainingPlots / num_x_plots);
            end
        end

        subplot(num_y_plots,num_x_plots,mod(q-1,total_plots)+1);
        t = 1:nFrames;
        plot(t,y{q},'g',t,y_est,'b');
        title(sprintf('C %.2f, R %.2f', correlations(q), rms(q)));
        set(gca, 'XTick', [], 'YTick', []);

    end

end

% Compute the error metrics
mean_correlation = mean(correlations);
mean_RMSE = mean(rms);
long_correlation = corr(y_predConcat, y_trueConcat).^2;

long_RMSE = sqrt(mean((y_predConcat - y_trueConcat).^2));
predictions = y_predConcat;
gts = y_trueConcat;

if(verbose)
    figure
    plot([1:numel(y_trueConcat)],y_trueConcat,'g',[1:numel(y_trueConcat)],y_predConcat,'b');
    title(sprintf('C %.2f, R %.2f', long_correlation, long_RMSE));
    set(gca, 'XTick', [], 'YTick', []);
end

end


function TorF = hasPQcap(gen, hilo)
%HASPQCAP  Checks for P-Q capability curve constraints.
%   TORF = HASPQCAP(GEN, HILO) returns a column vector of 1's and 0's. The 1's
%   correspond to rows of the GEN matrix which correspond to generators which
%   have defined a capability curve (with sloped upper and/or lower bound on
%   Q) and require that additional linear constraints be added to the OPF.
%
%   The GEN matrix in version 2 of the MATPOWER case format includes columns
%   for specifying a P-Q capability curve for a generator defined as the
%   intersection of two half-planes and the box constraints on P and Q. The
%   two half planes are defined respectively as the area below the line
%   connecting (Pc1, Qc1max) and (Pc2, Qc2max) and the area above the line
%   connecting (Pc1, Qc1min) and (Pc2, Qc2min).
%
%   If the optional 2nd argument is 'U' this function returns true only for
%   rows corresponding to generators that require the upper constraint on Q.
%   If it is 'L', only for those requiring the lower constraint. If the 2nd
%   argument is not specified or has any other value it returns true for rows
%   corresponding to gens that require either or both of the constraints.
%
%   It is smart enough to return true only if the corresponding linear
%   constraint is not redundant w.r.t the box constraints.

%   MATPOWER
%   Copyright (c) 2005-2016, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   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.

[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;

%% default value
if nargin < 2
    hilo = 'B';     %% look at both top and bottom by default
end

%% for which gens is it specified
k = find( gen(:, PC1) | gen(:, PC2) );
ng = size(gen, 1);

if isempty(k)
    TorF = zeros(ng, 1);
else
    %% eliminate cases where QMIN = QMAX = QC
    kk = find( gen(k, QMIN) == gen(k, QMAX) & ...
                gen(k, QMIN) == gen(k, QC1MAX) & ...
                gen(k, QMIN) == gen(k, QC1MIN) & ...
                gen(k, QMIN) == gen(k, QC2MAX) & ...
                gen(k, QMIN) == gen(k, QC2MIN) );
    k(kk) = [];

    %% check for errors in capability curve data
    if any( gen(k, PC1) >= gen(k, PC2) )
        error('hasPQcap: must have Pc1 < Pc2');
    end
    if any( gen(k, QC2MAX) <= gen(k, QC2MIN) & gen(k, QC1MAX) <= gen(k, QC1MIN) )
        error('hasPQcap: capability curve defines an empty set');
    end

    %% for which gens is it specified
    k = find( gen(:, PC1) ~= gen(:, PC2) );
    L = zeros(ng, 1);
    U = zeros(ng, 1);
    dPc = gen(k, PC2) - gen(k, PC1);

    if ~strcmp(hilo, 'U')       %% include lower constraint
        dQc = gen(k, QC2MIN) - gen(k, QC1MIN);
        Qmin_at_Pmin = gen(k, QC1MIN) + (gen(k, PMIN) - gen(k, PC1)) .* ...
            dQc ./ dPc;
        Qmin_at_Pmax = gen(k, QC1MIN) + (gen(k, PMAX) - gen(k, PC1)) .* ...
            dQc ./ dPc;
        L(k) = Qmin_at_Pmin > gen(k, QMIN) | Qmin_at_Pmax > gen(k, QMIN);
    end

    if ~strcmp(hilo, 'L')       %% include upper constraint
        dQc = gen(k, QC2MAX) - gen(k, QC1MAX);
        Qmax_at_Pmin = gen(k, QC1MAX) + (gen(k, PMIN) - gen(k, PC1)) .* ...
            dQc ./ dPc;
        Qmax_at_Pmax = gen(k, QC1MAX) + (gen(k, PMAX) - gen(k, PC1)) .* ...
            dQc ./ dPc;
        U(k) = Qmax_at_Pmin < gen(k, QMAX) | Qmax_at_Pmax < gen(k, QMAX);
    end

    TorF = L | U;
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


%this shows an example of using the "get axes" information from graph_picker

im=imread('example_graph.jpg');%load image
%display('identify reference points in graph picker and ''get axes''');
%[tmp]=graph_picker(im);

figure
image(im);%this is the same image that was used for graph_picker
axis off
h_imax=axes;%get handle to current axes
X=[2.722513e-01 8.272251e-01 1.204188e+00 1.612565e+00 2.146597e+00 2.774869e+00 3.591623e+00 4.219895e+00 ];%sample data
Y=[4.821429e+00 8.928571e+00 1.696429e+01 2.767857e+01 3.142857e+01 2.946429e+01 3.232143e+01 3.732143e+01 ];
plot(X,Y,'ob');
axis([-2.129e+00 6.055e+00 -1.191e+01 5.744e+01]);%this comes from graph_picker
set(h_imax,'color','none');%turn off background color
grid on


function [pvec, pstruct] = tapas_hgf_ar1_mab_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

l = r.c_prc.n_levels;

pvec(1:l)         = ptrans(1:l);                           % mu_0
pstruct.mu_0      = pvec(1:l);
pvec(l+1:2*l)     = exp(ptrans(l+1:2*l));                  % sa_0
pstruct.sa_0      = pvec(l+1:2*l);
pvec(2*l+1:3*l)   = tapas_sgm(ptrans(2*l+1:3*l),1);        % phi
pstruct.phi       = pvec(2*l+1:3*l);
pvec(3*l+1:4*l)   = ptrans(3*l+1:4*l);                     % m
pstruct.m         = pvec(3*l+1:4*l);
pvec(4*l+1:5*l-1) = exp(ptrans(4*l+1:5*l-1));              % ka
pstruct.ka        = pvec(4*l+1:5*l-1);
pvec(5*l:6*l-1)   = ptrans(5*l:6*l-1);                     % om
pstruct.om        = pvec(5*l:6*l-1);
pvec(6*l)         = exp(ptrans(6*l));                      % al
pstruct.al        = pvec(6*l);

return;


function [nodes2, edges2] = grMergeNodeClusters(nodes, edges)
%GRMERGENODECLUSTERS Merge cluster of connected nodes in a graph
%
%   grMergeNodeClusters(nodes, edges)
%   Detects groups of nodes that belongs to the same global node, and
%   replace them by a unique node. Coordinates of reference node is given
%   by the median coordinates of cluster nodes.
%
%   This function is intended to be used as filter after a binary image
%   skeletonization and vectorization.
%
%
%   See Also
%   grMergeNodesMedian
%

%   -----
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 13/08/2003.
%

%   HISTORY

%% Initialization

% intialize result
nodes2 = nodes;
edges2 = edges;

% compute degree of each node
degrees = grNodeDegree(1:size(nodes, 1), edges)';

% find index of multiple nodes
indMul = find(degrees > 2);

% indices of edges that link several multiple nodes
indEdges = sum(ismember(edges, indMul), 2) == 2;

% associate a label to each cluster
labels = grLabel(nodes, edges(indEdges, :));
clusterLabels = unique(labels(indMul));

%% Replace each cluster by median point

% iterate on clusters
for i = 1:length(clusterLabels)
    % indices of nodes of the current cluster
    inds = find(labels == clusterLabels(i));

    % coordinates of new reference node
    clusterNodes = nodes(inds, :);
    medianNode = median(clusterNodes, 1);

    % replace coordinates of reference node
    refNode = min(inds);
    nodes2(refNode, :) = medianNode;

    % replace node indices in edge array
    edges2(ismember(edges2, inds)) = refNode;
end

%% Clean up

% keep only relevant nodes
inds = unique(edges2(:));
nodes2 = nodes2(inds, :);

% relabeling of edges
for i = 1:length(inds)
    edges2(edges2 == inds(i)) = i;
end

% remove double edges
edges2 = unique(sort(edges2, 2), 'rows');

% remove 'loops'
edges2(edges2(:,1) == edges2(:,2), :) = [];


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 sim_2prob(value2trans) % autogenerated function wrapper
    % turned into function by Celso G Reyes 2017

    ZG=ZmapGlobal.Data; % used by get_zmap_globals

    report_this_filefun(mfilename('fullpath'));

    % needed variables
    % BigCatalog        big catalog from which to take the eqs randomly, produced by translating
    %                   consists of 100000 eqs
    % ni                number of earthquakes in a bin, i.e. sample size
    % NuBins            number of bins
    % BinLength         1/length(xt), length of shortest possible interval
    % winlen_days               length of interval in times shortest
    % NuRep             number of repetitions

    delta=winlen_days/NuBins;

    for nto=1:NuRep
        disp(nto);

        which=ceil(100000*(rand(ni)));
        for i=1:ni
            rancata(i)=BigCatalog(which(i));
        end
        clear i which;
        rancata=ceil(rancata*NuBins);

        for i=1:NuBins
            l=sum(rancata==i); Bins(i,1)=sum(l); clear l;
        end
        clear rancata i;

        FirstBin=ceil(rand(1)*(NuBins-winlen_days+1));

        zin=Bins(FirstBin:FirstBin+winlen_days-1); zout=[Bins(1:FirstBin-1,1); Bins(FirstBin+winlen_days:NuBins,1)];
        ToBeFitted(nto,1)=nto;
        % calculating beta
        ToBeFitted(nto,2)=(sum(zin)-ni*delta)/(sqrt(ni*delta*(1-delta)));
        % calculating z
        ToBeFitted(nto,3)=(mean(zout)-mean(zin))/(sqrt(var(zin)/sum(zin)+var(zout)/sum(zout)));
        clear Bins FirstBin zin zout;
    end
    clear BigCatalog nto delta;

    [meanval, std] =normfit(ToBeFitted(:,2)); IsFitted(1,1)=meanval; IsFitted(1,2)=std;
    [meanval, std] =normfit(ToBeFitted(:,3)); IsFitted(2,1)=meanval; IsFitted(2,2)=std;
    clear meanval std;
    clear ToBeFitted;

    switch value2trans
        case 'beta'
            Pbeta = normcdf(BetaValues,IsFitted(1,1),IsFitted(1,2));
            l = Pbeta == 0; Pbeta(l) = nan;
        case 'z'
            Pbeta = normcdf(BetaValues,IsFitted(2,1),IsFitted(2,2));
            l = Pbeta == 0; Pbeta(l) = nan;
    end

    % plot the resuts
    figure
    pq = -log10(1-Pbeta); l = isinf(pq);pq(l) = 18 ;
    pl1 = plot(xt,pq,'color',[0.0 0.5 0.9]);
    hold on
    l = pq < 1.3; pq(l) = nan;
    pl3 = plot(xt,pq,'b','Linewidth',2);

    pq = -log10(Pbeta);l = isinf(pq);pq(l) = 18 ;
    pl2 = plot(xt,pq,'color',[0.8 0.6 0.8]);
    l = pq < 1.3; pq(l) = nan;
    pl4 = plot(xt,pq,'r','Linewidth',2);

    maxd = [get(pl1,'Ydata') get(pl2,'ydata') ]; maxd(isinf(maxd)) = []; maxd = max(maxd);
    if maxd < 5 ; maxd = 5; end
    if isnan(maxd) == 1 ; maxd = 10; end

    legend([pl3 pl4],'Rate increases','Rate decreases');
    set(gca,'Ylim',[0 maxd+1])
    set(gca,'YTick',[1.3 2 3 4 5])
    set(gca,'YTickLabel',[ '    5%' ; '    1%' ;  '  0.1%' ;  ' 0.01%' ; '0.001%'])
    set(gca,'TickDir','out','Ticklength',[0.02 0.02],'pos',[0.2 0.2 0.7 0.7]);
    xlabel('Time [years]')
    ylabel('Significance level');
    set(gcf,'color','w')
    grid

    uicontrol('Units','normal',...
        'Position',[.8 .0 .1 .05],'String','Explain ... ',...
        'callback',@(~,~)showweb('explproba'));

    delete(probut)

end