function [sf,out2,out3] = symmetric_dirichlet_sym(in1,in2)
%SYMMETRIC_DIRICHLET_SYM
%    [SF,OUT2,OUT3] = SYMMETRIC_DIRICHLET_SYM(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.6.
%    24-Apr-2021 15:46:34

P1 = in2(:,1);
P2 = in2(:,2);
P3 = in2(:,3);
P4 = in2(:,4);
P5 = in2(:,5);
P6 = in2(:,6);
P7 = in2(:,7);
P8 = in2(:,8);
P9 = in2(:,9);
U1 = in1(:,1);
U2 = in1(:,2);
U3 = in1(:,3);
U4 = in1(:,4);
U5 = in1(:,5);
U6 = in1(:,6);
t2 = U1.*2.0;
t3 = U2.*2.0;
t4 = U3.*2.0;
t5 = U4.*2.0;
t6 = U5.*2.0;
t7 = U6.*2.0;
t8 = -P2;
t9 = -P3;
t10 = -P5;
t11 = -P6;
t12 = -P8;
t13 = -P9;
t15 = -U2;
t17 = -U3;
t20 = -U5;
t22 = -U6;
t24 = U1./2.0;
t25 = U2./2.0;
t26 = U3./2.0;
t27 = U4./2.0;
t28 = U5./2.0;
t29 = U6./2.0;
t14 = -t2;
t16 = -t3;
t18 = -t4;
t19 = -t5;
t21 = -t6;
t23 = -t7;
t30 = P1+t8;
t31 = P1+t9;
t32 = P4+t10;
t33 = P4+t11;
t34 = P7+t12;
t35 = P7+t13;
t36 = U1+t15;
t37 = U1+t17;
t38 = U4+t20;
t39 = U4+t22;
t40 = -t25;
t41 = -t26;
t42 = -t28;
t43 = -t29;
t44 = U2+U3+t14;
t45 = U5+U6+t19;
t46 = t30.^2;
t47 = t31.^2;
t48 = t32.^2;
t49 = t33.^2;
t50 = t34.^2;
t51 = t35.^2;
t52 = t2+t16;
t53 = t2+t18;
t54 = t5+t21;
t55 = t5+t23;
t56 = t36.^2;
t57 = t37.^2;
t58 = t38.^2;
t59 = t39.^2;
t66 = t24+t40;
t67 = t24+t41;
t68 = t25+t41;
t69 = t27+t42;
t70 = t27+t43;
t71 = t28+t43;
t72 = t30.*t31;
t73 = t32.*t33;
t74 = t34.*t35;
t75 = t36.*t37;
t76 = t38.*t39;
t77 = (t30.*t33)./2.0;
t78 = (t31.*t32)./2.0;
t79 = (t30.*t35)./2.0;
t80 = (t31.*t34)./2.0;
t81 = (t32.*t35)./2.0;
t82 = (t33.*t34)./2.0;
t83 = (t36.*t39)./2.0;
t84 = (t37.*t38)./2.0;
t60 = t46.*2.0;
t61 = t47.*2.0;
t62 = t48.*2.0;
t63 = t49.*2.0;
t64 = t50.*2.0;
t65 = t51.*2.0;
t85 = -t78;
t86 = -t80;
t87 = -t82;
t88 = -t84;
t89 = t56+t58;
t90 = t57+t59;
t91 = t46+t48+t50;
t92 = t47+t49+t51;
t93 = t75+t76;
t110 = t72+t73+t74;
t94 = t60+t62+t64;
t95 = t61+t63+t65;
t96 = t77+t85;
t97 = t79+t86;
t98 = t81+t87;
t99 = t83+t88;
t100 = t53.*t91;
t101 = t52.*t92;
t102 = t55.*t91;
t103 = t54.*t92;
t111 = t90.*t91;
t112 = t89.*t92;
t104 = t96.^2;
t105 = t97.^2;
t106 = t98.^2;
t107 = 1.0./t99.^2;
t108 = 1.0./t99.^3;
t113 = t94+t95;
t114 = t100+t101;
t115 = t102+t103;
t116 = t111+t112;
t109 = t107.^2;
t117 = t104+t105+t106;
t118 = 1.0./sqrt(t117);
t121 = t107.*t117;
t119 = (t91.*t118)./2.0;
t120 = (t92.*t118)./2.0;
t122 = t121+1.0;
t123 = (t94.*t118)./4.0;
t124 = (t95.*t118)./4.0;
t127 = (t100.*t118)./4.0;
t128 = (t101.*t118)./4.0;
t129 = (t102.*t118)./4.0;
t130 = (t103.*t118)./4.0;
t135 = t110.*t118;
t145 = (t113.*t118)./4.0;
t148 = (t114.*t118)./4.0;
t149 = (t115.*t118)./4.0;
t150 = (t116.*t118)./4.0;
t125 = -t123;
t126 = -t124;
t131 = -t127;
t132 = -t128;
t133 = -t129;
t134 = -t130;
t136 = -t135;
t137 = t135./2.0;
t147 = t93.*t135.*(-1.0./2.0);
t151 = t119.*t122;
t152 = t120.*t122;
t156 = t122.*t135.*(-1.0./2.0);
t173 = t66.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t174 = t67.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t175 = t68.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t176 = t66.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t177 = t67.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t178 = t68.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t179 = t70.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t180 = t71.*t108.*t117.*(t127-(t36.*t135)./2.0).*-2.0;
t181 = t69.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t182 = t71.*t108.*t117.*(t128-(t37.*t135)./2.0).*-2.0;
t183 = t67.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t184 = t68.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t185 = t66.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t186 = t68.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t187 = t69.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t188 = t70.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t189 = t71.*t108.*t117.*(t129-(t38.*t135)./2.0).*-2.0;
t190 = t69.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t191 = t70.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t192 = t71.*t108.*t117.*(t130-(t39.*t135)./2.0).*-2.0;
t193 = t67.*t108.*t117.*(t127-(t36.*t135)./2.0).*2.0;
t194 = t67.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t195 = t71.*t108.*t117.*(t127-(t36.*t135)./2.0).*2.0;
t196 = t69.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t197 = t71.*t108.*t117.*(t128-(t37.*t135)./2.0).*2.0;
t198 = t67.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t199 = t69.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t200 = t71.*t108.*t117.*(t129-(t38.*t135)./2.0).*2.0;
t201 = t69.*t108.*t117.*(t130-(t39.*t135)./2.0).*2.0;
t202 = t71.*t108.*t117.*(t130-(t39.*t135)./2.0).*2.0;
t203 = -t122.*(t135-t145);
t138 = -t137;
t139 = t36.*t137;
t140 = t37.*t137;
t141 = t38.*t137;
t142 = t39.*t137;
t143 = t44.*t137;
t144 = t45.*t137;
t146 = t93.*t137;
sf = -t122.*(t146-t150);
if nargout > 1
    t155 = t122.*t137;
    t157 = t125+t137;
    t158 = t126+t137;
    t163 = t136+t145;
    t166 = t147+t150;
    t153 = t119+t138;
    t154 = t120+t138;
    t159 = t131+t139;
    t160 = t132+t140;
    t161 = t133+t141;
    t162 = t134+t142;
    t164 = t143+t148;
    t165 = t144+t149;
    t171 = -t122.*(t123+t138);
    t172 = -t122.*(t124+t138);
    t219 = -t108.*t117.*(t146-t150);
    t220 = t108.*t117.*(t146-t150);
    out2 = [t122.*t164+t71.*t220.*2.0;t70.*t220.*-2.0-t122.*(t128-(t37.*t135)./2.0);t69.*t220.*2.0-t122.*(t127-(t36.*t135)./2.0);t122.*t165-t68.*t220.*2.0;t67.*t220.*2.0-t122.*(t130-(t39.*t135)./2.0);t66.*t220.*-2.0-t122.*(t129-(t38.*t135)./2.0)];
end
if nargout > 2
    t221 = t66.*t67.*t109.*t117.*(t146-t150).*-6.0;
    t222 = t66.*t68.*t109.*t117.*(t146-t150).*-6.0;
    t223 = t67.*t68.*t109.*t117.*(t146-t150).*-6.0;
    t224 = t66.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t225 = t66.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t226 = t67.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t227 = t66.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t228 = t67.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t229 = t68.*t69.*t109.*t117.*(t146-t150).*-6.0;
    t230 = t67.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t231 = t68.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t232 = t68.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t233 = t69.*t70.*t109.*t117.*(t146-t150).*-6.0;
    t234 = t69.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t235 = t70.*t71.*t109.*t117.*(t146-t150).*-6.0;
    t236 = t66.*t67.*t109.*t117.*(t146-t150).*6.0;
    t237 = t67.*t68.*t109.*t117.*(t146-t150).*6.0;
    t238 = t66.*t69.*t109.*t117.*(t146-t150).*6.0;
    t239 = t66.*t71.*t109.*t117.*(t146-t150).*6.0;
    t240 = t67.*t70.*t109.*t117.*(t146-t150).*6.0;
    t241 = t68.*t69.*t109.*t117.*(t146-t150).*6.0;
    t242 = t68.*t71.*t109.*t117.*(t146-t150).*6.0;
    t243 = t69.*t70.*t109.*t117.*(t146-t150).*6.0;
    t244 = t70.*t71.*t109.*t117.*(t146-t150).*6.0;
    t167 = t122.*t153;
    t168 = t122.*t154;
    t204 = t66.*t108.*t117.*t164.*2.0;
    t205 = t67.*t108.*t117.*t164.*2.0;
    t206 = t68.*t108.*t117.*t164.*2.0;
    t207 = t69.*t108.*t117.*t164.*2.0;
    t208 = t70.*t108.*t117.*t164.*2.0;
    t209 = t66.*t108.*t117.*t165.*2.0;
    t210 = t67.*t108.*t117.*t165.*2.0;
    t211 = t69.*t108.*t117.*t165.*2.0;
    t212 = t70.*t108.*t117.*t165.*2.0;
    t213 = t71.*t108.*t117.*t165.*2.0;
    t245 = t173+t199+t238;
    t246 = t191+t194+t240;
    t248 = t156+t179+t196+t243;
    t249 = t156+t185+t198+t236;
    t250 = t193+t201+t219+t226;
    t251 = t176+t188+t220+t225;
    t169 = -t167;
    t170 = -t168;
    t214 = -t205;
    t215 = -t207;
    t216 = -t210;
    t217 = -t211;
    t218 = -t213;
    t252 = t178+t212+t219+t231;
    t253 = t200+t204+t219+t239;
    t247 = t206+t218+t242;
    t254 = t202+t214+t220+t230;
    t255 = t175+t217+t220+t241;
    out3 = [t203-t71.*t108.*t117.*t164.*4.0-t71.^2.*t109.*t117.*(t146-t150).*6.0;t172+t197+t208+t244;t171+t195+t215+t234;t247;t254;t253;t170+t197+t208+t244;t152-t70.^2.*t109.*t117.*(t146-t150).*6.0-t70.*t108.*t117.*(t128-(t37.*t135)./2.0).*4.0;t248;t252;t246;t251;t169+t195+t215+t234;t248;t151-t69.^2.*t109.*t117.*(t146-t150).*6.0+t69.*t108.*t117.*(t127-(t36.*t135)./2.0).*4.0;t255;t250;t245;t247;t252;t255;t203+t68.*t108.*t117.*t165.*4.0-t68.^2.*t109.*t117.*(t146-t150).*6.0;t172+t186+t216+t237;t171+t184+t209+t222;t254;t246;t250;t170+t186+t216+t237;t152-t67.^2.*t109.*t117.*(t146-t150).*6.0+t67.*t108.*t117.*(t130-(t39.*t135)./2.0).*4.0;t249;t253;t251;t245;t169+t184+t209+t222;t249;t151-t66.^2.*t109.*t117.*(t146-t150).*6.0-t66.*t108.*t117.*(t129-(t38.*t135)./2.0).*4.0];
end


% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% 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
% any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [ff td ts wd err] = solve_projective(gyro, frame_time, W, video_file)

gyro(:,[2 1]) = gyro(:,1:2);    % x & y are swapped
gyro(:,3) = -gyro(:,3);         % rotations about z
dft = diff(frame_time);
dT = diff(gyro(:,4));
theta = ((gyro(1:end-1,1:3) + gyro(2:end,1:3)) / 2) .* dT(:,[1 1 1]);
theta = [0 0 0; cumsum(theta, 1)];

ff = -1572;
td = -0.1564;
ts = 0;

alpha_f = 1e-12;
alpha_td = 1e-12;
alpha_ts = 1e-12;
alpha_wd = 1e-10;
wd = -mean(gyro(:,1:3),1);
%d = [0 0 0];        % gyro drift

% read corresponding movie
xyloObj = mmreader(['data/' video_file '.mov']);
num_frames = xyloObj.NumberOfFrames;
vh = xyloObj.Height;
vw = xyloObj.Width;

frame1 = read(xyloObj, 200);
frame2 = read(xyloObj, 201);

for iter = 1:5000
    for f=100:num_frames-1
        dE_df = 0;
        dE_dtd = 0;
        dE_dts = 0;

        err = 0;
        denom = 0;

        x = W(f).x1;
        y = W(f).x2;
        for i = 1:size(x,1)
            K = [1 0 -vw/2; 0 1 -vh/2; 0 0 ff];

            X = K*[x(i,:)'; 1];
            Y = K*[y(i,:)'; 1];

            tx = frame_time(f) - td - ts * X(2) / vh;
            ty = frame_time(f+1) - td - ts * Y(2) / vh;

            w = interp1(gyro(:,4), gyro(:,1:3), [tx ty], 'linear', 'extrap');
            dw = diff(w);
            dws = (w(2,:) * Y(2) - w(1,:) * X(2)) / vh;
            dth = diff(interp1(gyro(:,4), theta(:,1:3), [tx ty], 'linear', 'extrap'));

            Rx = [1            0           0;
                  0 cos(dth(1)) -sin(dth(1));
                  0 sin(dth(1))  cos(dth(1))];

            Ry = [ cos(dth(2)) 0 sin(dth(2));
                   0           1           0;
                  -sin(dth(2)) 0 cos(dth(2))];

            Rz = [cos(dth(3)) -sin(dth(3)) 0;
                  sin(dth(3))  cos(dth(3)) 0;
                  0           0            1];

            dRx =[0             0           0;
                  0 -sin(dth(1)) -cos(dth(1));
                  0 cos(dth(1))  -sin(dth(1))];

            dRy =[-sin(dth(2)) 0  cos(dth(2));
                   0           0            0;
                  -cos(dth(2)) 0 -sin(dth(2))];

            dRz =[-sin(dth(3)) -cos(dth(3)) 0;
                   cos(dth(3)) -sin(dth(3)) 0;
                   0           0            0];

            R = Rz * Ry * Rx;
            RY = R * Y;

            XXY_XYX = X'*X*RY' - X'*RY*X';
            dE_dts = dE_dts + 2 * (XXY_XYX) * (Rz * Ry * dRx * dws(1) + Rz * dRy * Rx * dws(2) + dRz * Ry * Rx * dws(3)) * Y;
            dE_dtd = dE_dtd + 2 * (XXY_XYX) * (Rz * Ry * dRx * dw(1) + Rz * dRy * Rx * dw(2) + dRz * Ry * Rx * dw(3)) * Y;
            dE_df = dE_df + 2*(RY'*RY)*X(3)*ff + 2*(X'*X)*(RY'*(R*[0;0;f])) - 2*(X'*RY)*(ff*RY(3) + X'*(R*[0;0;f]));
            %{
            if f == 200 && mod(iter-1,20) == 0
                figure(1); clf;
                h1 = imshow(frame2); hold on;
                h2 = imshow(imtransform(frame1, maketform('affine', M'), 'XData', [1 vid_width], 'YData', [1 vid_height]));
                set(h2, 'AlphaData', 0.6);
                hold off;
                pause(0.01);
            end
            %}
            err = err + (X'*X)*(Y'*Y) - (X'*RY)^2;
            denom = denom + 1;
        end

        err = err / denom;

        ff = ff + alpha_f * dE_df;
        %td = td - alpha_td * dE_dtd;
        %ts = ts - alpha_ts * dE_dts;

        fprintf('f: %f, td: %f, ts: %f, d: (%f, %f, %f), error: %f\n', ff, td, ts, 0, 0, 0, err);
    end
end


function F = triangles_from_strip(S)
  % TRIANGLES_FROM_STRIP Create a list of triangles from a stream of indices
  % along a strip.
  %
  % Inputs:
  %   S  #S list of indices
  % Outputs:
  %   F  #S-2 by 3 list of triangle indices
  %

  %F = zeros(numel(S)-2,3);
  %for s = 3:numel(S)
  %  if mod(s,2) == 0
  %    F(s-2,:) = fliplr([S(s-2) S(s-1) S(s)]);
  %  else
  %    F(s-2,:) = [S(s-2) S(s-1) S(s)];
  %  end
  %end

  S = S(:);
  % Maintain order and orientations
  F = [ ...
    S(1:2:end-2) S(2:2:end-1) S(3:2:end) ...
    S(3:2:end-1) S(2:2:end-2) S(4:2:end)];
  F = reshape(F',3,numel(S)-2)';

end


function gpcf = gpcf_covar(varargin)
%   GPCF_COVAR creates a gpstuff covariance structure for the multivariate
%   Gaussian Process model based on the coregionalization models (LMC).
%   See Gelfand et al. (2004) in nonstationary multivariate process modelling
%   through spatially varying coregionalization. Sociedad de Estadistica e
%   Investigacion Operativa, Test.
%
%   The coregionalization matrix is a covariance matrix and its
%   parametrization is given via variances and the correlation matrix. The
%   correlation matrix is parametrizated with the transformation proposed by
%   Lewandowski et al. (2009) in generating correlation matrices based on
%   vines and extended onion method. Journal of multivariate analysis.
%
%   Description :
%
%   ─ GPCF = GPCF_COVAR('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%     creates correlation matrix structure in which
%     the named parameters have the specified values. Any
%     unspecified parameters are set to default values.
%
%   ─ GPCF = GPCF_COVAR(GPCF,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a covariance function structure with the named
%    parameters altered with the specified values.
%
%   ─ Parameters (of the gpstuff structure) for the covariance structure [default]
%     R_prior             - prior for correlation matrix  [prior_corrunif()]
%     V_prior             - prior for the variances. Must be a structure
%                           with each component being also a structure (the
%                           priors). Otherwise, just use prior_fixed.
%     corrFun             - structure containing other covariance
%                           functions. More specifically, each element of
%                           the structure must be a correlation function
%                           (σ²_f = 1). Each element of the structure can
%                           also be a combination of covariance functions,
%                           which will result to a correlation function.
%                           You always need to follow this structure when
%                           using that specific type of covariance function
%                           in the gpstructure (gp_set).
%
%     numberClass         - number of classes (GPs, categories, species, ...)
%                           being modelled (this is required to initialize
%                           other parameters)
%
%     degreeFreedom_prior - prior for degree of freedoms 'nu' [prior_corrunif()]
%     classVariables      - value defining which column of x is used to
%                           identify the class variables. If this is not
%                           given returns an error. They have to be given
%                           increasing downwards, otherwise you get an
%                           error.
%
%   »» Note ««
%   If the prior is 'prior_fixed' then the parameter in
%   question is considered fixed and it is not handled in
%   optimization, grid integration, MCMC etc.
%
% * See also
%   GP_SET, GPCF_*, PRIOR_*
%
% ─────────────── 2018 Marcelo Hartmann.

% If you use this file. Learn to recognize its author. Copyright is good but
% misleading.

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  if nargin > 0 && ischar(varargin{1}) && ismember(varargin{1}, {'init' 'set'})
    % remove init and set if isfield(gpcf,'metric')
    varargin(1) = [];
  end

  ip = inputParser;
  ip.FunctionName = 'GPCF_COVAR';
  ip.addOptional('gpcf', [], @isstruct);
  ip.addParamValue('R', 0.01, @(x) isvector(x) && ~any(abs(x) > 1));
  ip.addParamValue('V', 1, @(x) isvector(x) && ~any(x < 0));
  ip.addParamValue('R_prior', prior_corrunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('V_prior', prior_t('s2', 2), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('corrFun', {}, @(x) ~isempty(x) && iscell(x));
  ip.addParamValue('classVariables', [], @(x) ~isempty(x) && mod(x, 1) == 0 && x > 0);
  ip.addParamValue('numberClass', [], @(x) mod(x, 1) == 0 && x > 1);
  ip.addParamValue('aValue', 1, @(x) isreal(x) &&  x > 0);

  ip.parse(varargin{:});

  gpcf = ip.Results.gpcf;

  if isempty(gpcf)
      init = true;
      gpcf.type = 'gpcf_covar';
  else
      if ~isfield(gpcf, 'type') && ~isequal(gpcf.type, 'gpcf_covar')
          error('First argument does not seem to be a valid correlation structure')
      end
      init = false;
  end

  if init
      % Set the function handles to the subfunctions
      gpcf.fh.realtoz = @gpcf_covar_realtoz;
      gpcf.fh.ztoreal = @gpcf_covar_ztoreal;
      gpcf.fh.rhotoreal = @gpcf_covar_rhotoreal;
      gpcf.fh.realtorho = @gpcf_covar_realtorho;
      gpcf.fh.pak = @gpcf_covar_pak;
      gpcf.fh.unpak = @gpcf_covar_unpak;
      gpcf.fh.lp = @gpcf_covar_lp;
      gpcf.fh.lpg = @gpcf_covar_lpg;
      gpcf.fh.cfg = @gpcf_covar_cfg;
      gpcf.fh.cov = @gpcf_covar_cov;
      gpcf.fh.sigma = @gpcf_covar_sigma;
      gpcf.fh.trcov = @gpcf_covar_trcov;
      gpcf.fh.trvar = @gpcf_covar_trvar;
      gpcf.fh.recappend = @gpcf_covar_recappend;
  end

  % initialize class variables and selected variables
  if isempty(ip.Results.classVariables) || isempty(ip.Results.numberClass)
      error('class variables and number of classes must be given')
  end
  gpcf.classVariables = ip.Results.classVariables;
  gpcf.numberClass = ip.Results.numberClass;
  gpcf.vectSize = (gpcf.numberClass^2 - gpcf.numberClass)/2;
  gpcf.aValue = ip.Results.aValue;

  % create indexes which will easy our lives from now on
  seq = 1:gpcf.vectSize;
  i = ceil(0.5 + 0.5 * sqrt(1 + 8 * seq)); % ith column
  j = seq - (i - 2).*(i - 1)/2;            % jth column
  ind1 = (j - 1) * gpcf.numberClass + i;   % linear indexes for lower triangular
  ind2 = (i - 1) * gpcf.numberClass + j;   % linear indexes for upper triangular
  gpcf.index = [i; j; ind1; ind2; seq]';

  % Initialize correlations functions
  if init || ~ismember('corrFun', ip.UsingDefaults)
      gpcf.corrFun = ip.Results.corrFun;
  end

  if length(gpcf.corrFun) ~= gpcf.numberClass
      error('the number of correlation functions must match the number of classes')
  end

  % Initialize correlations parameters
  if init && ~ismember('R', ip.UsingDefaults)
      gpcf.R = ip.Results.R;
      if length(gpcf.R) ~= gpcf.vectSize
          error('length of the vector does not agree with the number of classes')
      end
  elseif init
      gpcf.R = (-0.99 + 1.98.*rand(1, gpcf.vectSize)) * 0.001;
  end

  % Initialize variance parameters
  if init && ~ismember('V', ip.UsingDefaults)
      gpcf.V = ip.Results.V;
      if length(gpcf.V) ~= gpcf.numberClass
          error('length of the variance vector does not agree with the number of classes')
      end
  elseif init
      % takes one if nothing is given.
      gpcf.V = ones(1, gpcf.numberClass);
  end

  % initialize prior structure
  if init
      gpcf.p = [];
  end

  % initialize correlations
  if init || ~ismember('R_prior', ip.UsingDefaults)
    gpcf.p.R = ip.Results.R_prior;
    if ~isempty(gpcf.p.R)
        % corrects the prior
        gpcf.p.R.numberClass = gpcf.numberClass;
        gpcf.p.R.vectSize = gpcf.vectSize;
        gpcf.p.R.aValue = gpcf.aValue;
        if gpcf.p.R.numberClass > gpcf.p.R.nu
            error('degrees of freedom must be greater than number of classes')
        end
    end
  end

  % initialize variances
  if init && ~ismember('V_prior', ip.UsingDefaults)
      gpcf.p.V = ip.Results.V_prior;
      if ~isempty(ip.Results.V_prior)
          gpcf.varfields = (fields(gpcf.p.V)');
          if length(structfun(@numel, gpcf.p.V)) ~= gpcf.numberClass
              error('prior missing for some variance')
          end
      end
  else
      var = [repmat('var', gpcf.numberClass , 1), num2str((1:gpcf.numberClass)')];
      for ind = 1:size(var, 1)
          var_aux = var(ind, :);
          var_aux = var_aux(~isspace(var_aux));
          Vpriors.(var_aux).p = ip.Results.V_prior;
          %Vpriors.(var(ind, :)).p = ip.Results.V_prior;
      end
      gpcf.p.V = Vpriors;
      gpcf.varfields = (fields(gpcf.p.V)');
  end

end

function y = gpcf_covar_realtoz(x, a, d)
% Description :
%   · Transforms the real line to the interval (-1, 1) using the shifted
%   logistic function
%
%   · rho = 2 ./ (1 + exp(-a*x)) - 1;
%   · x is the vector on the real line
%   · d equals to [], 1 or 2, and indicates whether the output corresponds
%     no derivatives, first or second derivative.

if isempty(d)
    % modified logistic distribution function
    y = 2 ./ (1 + exp(-a*x)) - 1;
elseif d == 1
    % repeated terms
    eminax = exp(-a*x);
    % 1st derivative, drho/dx
    y = 2 * a * (1 + eminax).^(-2) .* eminax;
elseif d == 2
    % repeated terms
    a2 = a ^ 2;
    eminax = exp(-a*x);
    % 2nd derivative, d2rho/dx2
    y = 4 * a2 * (1 + eminax).^(-3) .* eminax.^2 - 2 * a2 * (1 + eminax).^(-2) .* eminax;
else
    error('Something is wrong. Maybe d should be [], 1 or 2')
end
end

function y = gpcf_covar_ztoreal(x, a)
%  Description :
%   · Transforms the interval (-1, 1) to the real line, using the inverse
%   hyperbolic tangent function
%
%   · y = - 1/a * log(2/(x+1) - 1)
%   · x in (-1, 1)^(dim(x))

if any(abs(x) >= 1)
    error('domain error');
else
    y = -1/a * log(2./(x+1) - 1);
end
end

function y = gpcf_covar_rhotoreal(gpcf)
%  Description :
%   · Transforms the matrix R to the matrix Z (composed by the elements
%   gamma on the real line)

ind1 = gpcf.index(:, 3);
ind2 = gpcf.index(:, 4);

Z = zeros(gpcf.numberClass, gpcf.numberClass);
R = eye(gpcf.numberClass);
R([ind1, ind2]) = [gpcf.R'; gpcf.R'];

W = chol(R);
Z(1, 2:gpcf.numberClass) = W(1, 2:gpcf.numberClass);
for i1 = 2:(gpcf.numberClass-1)
    for j1 = (i1+1):gpcf.numberClass
        Z(i1, j1) = W(i1, j1) .* exp(-0.5*sum(log(1-Z(1:(i1-1), j1).^2)));
        % Z(i1, j1) = W(i1, j1) .* 1/sqrt(prod(1-Z(1:(i1-1), j1).^2));
    end
end

y = gpcf.fh.ztoreal(Z(ind2), gpcf.aValue);
end

function y = gpcf_covar_realtorho(gpcf, w, d)
%  Description :
%   · Transforms the vector gamma on the real line to the matrix R (in a vector);

%   · z = 2 ./ (1 + exp(-a*x)) - 1;
%   · x is the vector on the real line
%   · d equals to [], 1 or 2, and indicates whether the output corresponds
%     no derivatives, first or second derivative.

i = gpcf.index(:, 1);
j = gpcf.index(:, 2);
ind2 = gpcf.index(:, 4);
seq = gpcf.index(:, end);
i2 = gpcf.vectSize;

Z = zeros(gpcf.numberClass, gpcf.numberClass);
W = eye(gpcf.numberClass, gpcf.numberClass);

Z(ind2) = gpcf.fh.realtoz(w(1:i2), gpcf.aValue, []);
W(1, 2:gpcf.numberClass) = Z(1, 2:gpcf.numberClass);

for i1 = 2:gpcf.numberClass
    for j1 = i1:gpcf.numberClass
        ztmp = 0.5.*sum(log(1 - Z(1:(i1-1), j1).^2));
        W(i1, j1) = ((i1 == j1) + (i1 ~= j1)*Z(i1, j1)) * exp(ztmp);
        % ztmp = sqrt(1-Z(1:(i1-1), j1).^2);
        % W(i1, j1) = ((i1 == j1) + ~(i1 == j1)*Z(i1, j1)) * prod(ztmp);
    end
end

if isempty(d)
    % correlation matrix
    R = W'*W;
    y = R(ind2);

elseif d == 1
    % indexes
    ind3 = [j i seq];

    % derivatives of the cholesk decomposition w.r.t values on the real line
    dW = zeros(gpcf.numberClass, gpcf.numberClass, i2);
    dL = zeros(gpcf.numberClass, gpcf.numberClass, i2);
    z = Z(ind2);
    dz = gpcf.fh.realtoz(w, gpcf.aValue, 1);

    fz = sqrt(1 - z.^2);
    dfz = - 1./sqrt(1 - z.^2) .*  z .* dz;

    for k = 1:i2
        % take the column
        j1 = ind3(ind3(:, 3) == k, 2);

        % take the indexes for that column
        ind4 = ind3(ind3(:, 2) == j1, end);

        fzaux = fz(ind4);
        dfzaux = dfz(ind4);

        for i1 = 1:j1
            if i1 == 1
                dW(i1, j1, k) = (k == ind4(1))*dz(k);

            elseif i1 ~= j1
                kaux = ind4(i1);

                if kaux < k
                    continue
                else
                    ind5 = ind4(1:(i1-1));
                    fzaux2 = fz(ind5);
                    dfzaux2 = dfz(ind5);
                    e = (k == ind5);

                    if  kaux == k
                        %dW(i1, j1, k) = dz(k).*prod(fzaux2(~e));
                        dW(i1, j1, k) = dz(k).*exp(sum(log(fzaux2(~e))));
                    end

                    if kaux > k
                        %dW(i1, j1, k) = z(kaux).*dfzaux2(e)*prod(fzaux2(~e));
                        dW(i1, j1, k) = z(kaux).*dfzaux2(e)*exp(sum(log(fzaux2(~e))));
                    end
                end

            elseif i1 == j1
                e = (k == ind4);
                %dW(i1, j1, k) = dfzaux(e)*prod(fzaux(~e));
                dW(i1, j1, k) = dfzaux(e)*exp(sum(log(fzaux(~e))));

            end
        end
        dL(:, :, k) = dW(:, :, k)';

    end
    y = dL;

elseif d == 2
    % lower cholesk decompostion of R
    y = W';

end
end

function [w, s, h] = gpcf_covar_pak(gpcf, w)
%  GPCF_COVAR_PAK Combine GP correlation matrix values into one vector
%   keeping the order of input below.
%
%  Description :
%   · W = GPCF_COVAR_PAK(GPCF) takes the non-diagonal elements of
%     the correlation matrix and put them into a single row vector w
%     This is a mandatory subfunction used for example
%     in energy and gradient computations.
%
%   · w = [rho_(1, 2), ..., rho_(1, J), rho_(2, 3), ..., rho_(2, J), ..., rho_((J-1), J)]
%
%  * See also
%    GPCF_covar_UNPAK

  w = []; s = {}; h = [];

  % pak parameters of correlation functions
  for i = 1:gpcf.numberClass
      [wh, sh, hh] = gpcf.corrFun{i}.fh.pak(gpcf.corrFun{i});
      w = [w wh];
      s = [s; sh];
      h = [h 1+hh];
  end

  % pak correlations
  if ~isempty(gpcf.p.R)
      i = gpcf.index(:, 1);
      j = gpcf.index(:, 2);
      waux = gpcf.fh.rhotoreal(gpcf);
      w = [w waux'];
      S = [repmat('realcorr.', gpcf.vectSize, 1), num2str(j), num2str(i)];
      s = [s; cellstr(S)];
      h = [h 1];

      % pak hyperparameters of R
      [wh, sh, hh] = gpcf.p.R.fh.pak(gpcf.p.R);
      sh = strcat(repmat('prior-', size(sh, 1), 1), sh);
      w = [w wh];
      s = [s; sh];
      h = [h 1+hh];
  end

  % pak variances
  if ~isempty(gpcf.p.V)
    w = [w log(gpcf.V)];
    S = [repmat('log var.', gpcf.numberClass, 1), num2str((1:gpcf.numberClass)')];
    s = [s; cellstr(S)];
    h = [h 1];

    % pak hyperparameters of V
    for i = 1:gpcf.numberClass
        cfvf = gpcf.varfields{i};
        [wh, sh, hh] = gpcf.p.V.(cfvf).p.fh.pak(gpcf.p.V.(cfvf).p);
        sh = strcat(repmat('prior-', size(sh, 1), 1), sh);
        w = [w wh];
        s = [s; sh];
        h = [h 1+hh];
    end

   end
end

function [gpcf, w] = gpcf_covar_unpak(gpcf, w)
%  GPCF_covar_UNPAK Sets the covariance function parameters into the
%   structure
%
%  Description :
%   · [GPCF, W] = GPCF_COVAR_UNPAK(GPCF, W) takes a covariance
%     function structure GPCF and a hyperparameter vector W,
%     and returns a covariance function structure identical to
%     the input, except that the covariance hyperparameters have
%     been set to the values in W. Deletes the values set to GPCF
%     from W and returns the modified W. This is a mandatory
%     subfunction used for example in energy and gradient computations.
%
%  * See also
%    GPCF_covar_PAK

  % unpak parameters of correlations functions
  for i = 1:gpcf.numberClass
      cf = gpcf.corrFun{i};
      [gpcf.corrFun{i}, w] = cf.fh.unpak(cf, w);
  end

  gpp = gpcf.p;

  % unpak for Σ
  if ~ isempty(gpp.R)
      gpcf.R = gpcf.fh.realtorho(gpcf, w(1:gpcf.vectSize), [])';
      w = w((gpcf.vectSize + 1):end);

      % unpak hyperparameters of R
      [p, w] = gpcf.p.R.fh.unpak(gpcf.p.R, w);
      gpcf.p.R = p;
  end

  if ~ isempty(gpp.V)
      i3 = gpcf.numberClass;
      gpcf.V = exp(w(1:i3));
      w = w((i3 + 1):end);

      % unpak hyperparameters of V
      % loop for the hyperpriors
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};
          [p, w] = gpcf.p.V.(cfvf).p.fh.unpak(gpcf.p.V.(cfvf).p, w);
          gpcf.p.V.(cfvf).p = p;
      end
  end
end

function lp = gpcf_covar_lp(gpcf)
%  GPCF_COVAR_LP Evaluate the log prior of covariance function parameters
%
%  Description :
%   · LP = GPCF_COVAR_LP(GPCF, X, T) takes a correlation function
%     structure GPCF and evaluates log-prior(R)
%
%  * See also
%    GPCF_COVAR_PAK, GPCF_covar_UNPAK, GPCF_covar_LPG, GP_E

  lp = 0;

  % add log-prior for correlations functions parameters
  for i = 1:gpcf.numberClass
      lp = lp + gpcf.corrFun{i}.fh.lp(gpcf.corrFun{i});
  end

  gpp = gpcf.p;

  % log-prior for correlations
  if ~isempty(gpp.R)
     % mapping to the real line
     x  = gpcf.fh.rhotoreal(gpcf);
     z  = gpcf.fh.realtoz(x, gpcf.aValue, []);
     dz = gpcf.fh.realtoz(x, gpcf.aValue, 1);

     % add log-prior on rho parameterization
     lp = lp + gpp.R.fh.lp(gpcf.R, gpp.R);

     % add log(|det J|) on the real line parameterization. Equation 11 in
     % Lewandowski et al (2009).
     pwr = gpcf.numberClass - gpcf.index(:, 2) - 1;
     lp = lp + 0.5 .* sum(pwr .* log(1 - z.^2)) + sum(log(dz));

  end

  % log-prior for variances
  if ~isempty(gpp.V)
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};

          lp = lp + gpp.V.(cfvf).p.fh.lp(gpcf.V(i), ...
              gpp.V.(cfvf).p) + log(gpcf.V(i));
      end
  end
end

function lpg = gpcf_covar_lpg(gpcf)
%  GPCF_COVAR_LPG Evaluate gradient of the log prior with respect
%   to the parameters.
%
%  Description :
%   · LPG = GPCF_COVAR_LPG(GPCF) takes a covariance function
%     structure GPCF and returns LPG = d log (p(th))/dth, where th
%     is the parametric vector of correlations and variances.
%     This is a mandatory subfunction used for example in gradient
%     computations.
%
%  * See also
%    GPCF_covar_PAK, GPCF_covar_UNPAK, GPCF_covar_LP, GP_G

  lpg = [];

  % stack grad vector of the correlation functions parameters
  for i = 1:gpcf.numberClass
      lpg = [lpg gpcf.corrFun{i}.fh.lpg(gpcf.corrFun{i})];
  end

  gpp = gpcf.p;

  % for correlations
  if ~isempty(gpcf.p.R)
      % mapping to the real line
      real = gpcf.fh.rhotoreal(gpcf);
      z   = gpcf.fh.realtoz(real, gpcf.aValue, [])';
      dz  = gpcf.fh.realtoz(real, gpcf.aValue, 1)';
      d2z = gpcf.fh.realtoz(real, gpcf.aValue, 2)';
      pwr = (gpcf.numberClass - gpcf.index(:, 2) - 1)';

      % cholesk decomposition
      L = gpcf.fh.realtorho(gpcf, real, 2);

      % transforming to first derivative of L w.r.t gammas
      dL = gpcf.fh.realtorho(gpcf, real, 1);

      b = zeros(gpcf.vectSize, gpcf.vectSize);
      for k = 1:size(dL, 3)
          dRaux = dL(:, :, k)*L' + L*dL(:, :, k)';
          b(:, k) = dRaux(gpcf.index(:, 4));
      end

      % dlogp(R(gamma))/dgamma
      dlpdgm = gpp.R.fh.lpg(gpcf.R, gpp.R) * b;

      lpgs = dlpdgm - pwr.*(z.*dz./(1 - z.^2)) + d2z./dz;
      lpg = [lpg lpgs];
  end

  % for variances
  if ~isempty(gpp.V)
      % building the grad vector on the real line parameterization
      for i = 1:gpcf.numberClass
          cfvf = gpcf.varfields{i};
          lpgs = gpp.V.(cfvf).p.fh.lpg(gpcf.V(i), gpp.V.(cfvf).p);
          lpg = [lpg lpgs(1).*gpcf.V(i)+1 lpgs(2:end)];
      end
  end
end

function C = gpcf_covar_sigma(gpcf, type)
%  GP_COVAR_COV  Evaluate the covariance matrix Σ of the linear model of
%  coregionalization and its Cholesky decomposition.
%
%  Description :
%   · C = GP_COVAR_COV(GP, i1) takes the gpcf structure and
%     returns the covariance matrix Σ and its Cholesky decomposition.
%     Every element (i, j) of Σ contains the covariance (parameterised by
%     correlation and variances) between class i and class j. This is a
%     mandatory subfunction.
%
%  * See also
%    GPCF_COVAR_TRCOV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV

% linear indexes
ind1 = gpcf.index(:, 3);
ind2 = gpcf.index(:, 4);

% corr matrix
R = eye(gpcf.numberClass);

% filling elements in lower and upper part
R([ind1, ind2]) = [gpcf.R, gpcf.R];

% matrix Σ
Sig = bsxfun(@times, bsxfun(@times, sqrt(gpcf.V)', R), sqrt(gpcf.V));
% Sig = S * R * S;

% other options
if nargin == 2
    switch type
        case 'cov'
            % empty matrix
            C{1} = Sig;

            % calcule cholesk decomposition of Sigma
            [L, notpositivedefinite] = chol(Sig, 'lower');
            % test whether is positive-definite or not
            if notpositivedefinite
                L = NaN(gpcf.numberClass, gpcf.numberClass);
                L(ind2) = 0;
            end

            % take cholesky decomposition
            C{2} = L;

        case 'corr'
            x = gpcf.fh.rhotoreal(gpcf);
            L = gpcf.fh.realtorho(gpcf, x, 2);

            C{1} = L*L';
            C{2} = L;
    end
else
    C = Sig;

end
end

function C = gpcf_covar_cov(gpcf, x1, x2)
%  GP_COVAR_COV  Evaluate the covariance matrix between two input vectors
%
%  Description
%   · C = GP_COVAR_COV(GP, TX, X) takes in correlation structure
%     and two matrixes TX and X that contain input vectors to GP.
%     Returns covariance matrix C.
%     Every element ij of C contains correlation between inputs i
%     in TX and j in X. This is a mandatory subfunction used for
%     example in prediction and energy computations.
%
%
%  * See also
%    GPCF_COVAR_TRCOV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV

 if isempty(x2)
     x2 = x1;
 end

 if size(x1, 2) ~= size(x2, 2)
     error('the number of columns of X1 and X2 has to be same')
 end

 xClass1 = x1(:, gpcf.classVariables);
 xClass2 = x2(:, gpcf.classVariables);

 if ~issorted(xClass1) || ~issorted(xClass2)
     error('you need to give the class variable increasing downwards');
 end

 if max(xClass1) > gpcf.numberClass || max(xClass2) > gpcf.numberClass
     error('more classes than the given number of classes');
 end

 n1 = size(xClass1, 1);
 n2 = size(xClass2, 1);

 % information of the classes in the data
 ind1 = unique(xClass1)'; nind1 = size(ind1, 2);
 ind2 = unique(xClass2)'; nind2 = size(ind2, 2);

 % positions of observations in each vector
 nb1 = find(diff([-inf xClass1' inf]));
 nb2 = find(diff([-inf xClass2' inf]));

 % chol(Σ)
 M = gpcf.fh.sigma(gpcf, 'cov');
 L = M{2};

 % M = gpcf.fh.sigma(gpcf, 'corr');
 % L = bsxfun(@times, sqrt(gpcf.V)', M{2});

 C = zeros(n1, n2);
 for k = 1:gpcf.numberClass

     % kth column of the cholesk decomposition of Σ = SRS
     T = L(:, k) * L(:, k)';

     for i = 1:nind1
         for j = 1:nind2
             inb1 = nb1(i) : nb1(i + 1) - 1;
             inb2 = nb2(j) : nb2(j + 1) - 1;
             K(inb1, inb2) = T(ind1(i), ind2(j));
         end
     end

     cf = gpcf.corrFun{k};
     C = C + cf.fh.cov(cf, x1, x2) .* K;
end

 C(abs(C) < eps) = 0;
end

function dCRV = gpcf_covar_cfg(gpcf, x, x2, mask, i1)
%  GPCF_COVAR_CFG  Evaluate the derivarive of the covar matrix
%
%  Description :
%   · dRff = GPCF_COVAR_CFG(GPCF, X) takes a
%     covariance structure GPCF, a matrix of inputs
%     vectors and returns dSRS/drho and/or dSRS/dV, the gradients of
%     SRS matrix
%
%   · dRff = GPCF_COVAR_CFG(GPCF, X, X2) takes a
%     covariance structure GPCF, a matrix X of input
%     vectors and returns  dSRS/drho and/or dSRS/dV, the gradients of
%     SRS matrix with respect to rho (cell array with matrix
%     elements). This subfunction is needed when using sparse
%     approximations (e.g. FIC).
%
%  * See also
%    GPCF_COVAR_PAK, GPCF_COVAR_UNPAK, GPCF_COVAR_LP, GP_G

dCRV = {};

% covariance matrix Σ and chol(Σ)
M = gpcf.fh.sigma(gpcf, 'corr'); % R = M{1};
L = M{2};

% covariance matrix
%Sig = sVL * sVL';

% \sqrt(diag(var)) times L
sVL = bsxfun(@times, sqrt(gpcf.V)', L);

% take class variables
xC = x(:, gpcf.classVariables);

% positions of the observations
nb = diff([0 xC' xC(end) + 1]) .* (1:(length(xC) + 1));
nb = nb(nb ~= 0);

% number of observation in each class
diffnb = diff(nb);

% number of classes
nc = gpcf.numberClass;

% number of observations
n = length(xC);

% use savememory option
savememory = nargin == 5;

if nargin == 2 || (isempty(x2) && isempty(mask))

    % Evaluate the values (dSRS/dgamma_12, dSRS/dgamma_13, ..., dSRS/dgamma_(J-1)J
    % and/or dSRS/dV_1 ...  dSRS/dV_J

    % build the big Tj's
    T = cell(1, gpcf.numberClass);

    for  k = 1:gpcf.numberClass
        Taux = sVL(:, k) * sVL(:, k)';

        for i = 1:gpcf.numberClass
            inb1 = nb(i) : nb(i + 1) - 1;
            T{k}(inb1, inb1) = Taux(i, i);

            for j = (i + 1):gpcf.numberClass
                inb2 = nb(j):nb(j + 1) - 1;

                T{k}(inb1, inb2) = Taux(i, j);
                T{k}(inb2, inb1) = Taux(j, i);
            end
        end
    end

    % correlations matrices
    dC = {};

    for i1 = 1:nc
        dKK = gpcf.corrFun{i1}.fh.cfg(gpcf.corrFun{i1}, x);

        if ~isempty(dKK)
            % number of derivatives of the correlation function
            i2 = length(dKK);

            % derivatives of the specific correlation function
            for i3 = 1:i2
                dC{end + 1} = dKK{i3} .* T{i1};
            end
        end
    end

    % correlations and variances in Σ = sqrt(S) L L sqrt(S)
    sVdL = [];
    dsVL = [];

    if ~isempty(gpcf.p.R) || ~isempty(gpcf.p.V)

        % for gammas in Σ
        if ~isempty(gpcf.p.R)
            % transforming to the real line
            real = gpcf.fh.rhotoreal(gpcf);

            % transforming to first derivative of L w.r.t gammas
            dL = gpcf.fh.realtorho(gpcf, real, 1);

            % variances times first derivatives w.r.t correlations
            % \sqrt(diag(var)) (dLj/dΘ)
            sVdL = bsxfun(@times, sqrt(gpcf.V)', dL);
        end

        % for variances in Σ
        if ~isempty(gpcf.p.V)
            dV = 0.5 .* sqrt(gpcf.V);
            dsVLm = bsxfun(@times, dV', L); % take the lines ...
            dsVL = zeros(nc, nc, nc);

            for k = 1:nc
                dsVL(k, :, k) = dsVLm(k, :);
            end
        end

        % derivatives w.r.t to correlations or/and variances
        dSig = cat(3, sVdL, dsVL);

        if ~isempty(dSig)

            nSig = length(dSig);
            dL = cell(1, nSig);
            % dCorr = zeros(n, n);

            for k = 1:nSig
                % sum_j R_j kron (daj/dΘ * aj' + aj * daj/dΘ')
                dL{k} = zeros(n, n);

                for j = 1:nc
                    aj  = repelem(sVL(:, j).', diffnb);
                    daj = repelem(dSig(:, j, k), diffnb);
                    dd = daj * aj;

                    cf = gpcf.corrFun{j};
                    dCorr = cf.fh.trcov(cf, x) .* (dd + dd.');
                    dL{k} = dL{k} + dCorr;
                end
            end
        end
    end

    % return matrices
    dCRV = [dC dL];

% related to sparse approximations
elseif nargin == 3 || isempty(mask)
    error('nargin == 3 || isempty(mask) not implemented yet');

elseif nargin == 4 || nargin == 5
    error('nargin == 4 || nargin == 5 not implemented yet');

end
end

function C = gpcf_covar_trcov(gpcf, x)
%  GP_COVAR_TRCOV  Evaluate training covariance matrix of inputs
%
%  Description :
%   · C = GP_COVAR_TRCOV(GP, TX) takes in correlation structure
%     matrix TX that contains training input vectors.
%     Returns covariance matrix C. Every element ij of C contains
%     the correlation between inputs i and j in TX.
%     This is a mandatory subfunction used for example in
%     prediction and energy computations.
%
%  * See also
%    GPCF_COVAR_COV, GPCF_COVAR_TRVAR, GP_COV, GP_TRCOV
%
% HERE IS TO TAKE ADVANTAGE OF THE SYMMETRIC MATRIX

% takes the class variables
xC = x(:, gpcf.classVariables);

% checking
if ~issorted(xC)
    error('class variable should increasing downwards');
end

% number of observations
n = size(xC, 1);

% getting the information of the classes in the data
a = unique(xC); % na = size(a, 1);

% checking
if max(a) > gpcf.numberClass
% if na ~= gpcf.numberClass || max(a) ~= gpcf.numberClass
    error('more or less classes than given to the model');
end

% getting the information of the classes in the data
ind = unique(xC)'; nind = size(ind, 2);

% number of observations in each class
nb = find(diff([-inf xC' inf]));

% Try to use the C implementation
% C = trcov_corr(gpcf, x1);

C = NaN;

if isnan(C)
    % take chol(Σ)
    M = gpcf.fh.sigma(gpcf, 'cov');
    L = M{2};

    % full matrices
    C = zeros(n, n);  K = zeros(n, n);

    for k = 1:gpcf.numberClass
        % T_k = a_k * a_k' matrices,
        T =  L(:, k) * L(:, k)';

        for i = 1:nind
            inb1 = nb(i) : nb(i + 1) - 1;
            K(inb1, inb1) = T(ind(i), ind(i));

            for j = (i + 1):nind
                inb2 = nb(j) : nb(j + 1) - 1;

                K(inb1, inb2) = T(ind(i), ind(j));
                K(inb2, inb1) = T(ind(j), ind(i));
            end
        end

        % corrFun kronecker T_k
        cf = gpcf.corrFun{k};
        C = C + cf.fh.trcov(cf, x) .* K;
    end

    C(abs(C) < eps) = 0;
end
end

function C = gpcf_covar_trvar(gpcf, x)
%  GP_COVAR_VECTOR  Evaluate training variance vector
%
%  Description:
%   · C = GP_COVAR_TRVAR(GPCF, TX) takes in correlation structure
%     of and matrix TX that contains training inputs.
%     Returns correlation vector C, which are ones.
%     Every element i of C contains the correlation of the input in TX.
%     This is a mandatory subfunction used for example in prediction and
%     energy computations.
%
%  * See also:
%    GPCF_COVAR_COV, GP_COV, GP_TRCOV

 % take indicator class
 x = x(:, gpcf.classVariables);

 % information of the classes in the data
 a = unique(x);

 % locate each class
 nb = find(diff([-inf x' inf]));

 % number of observation in each class
 diffnb = diff(nb);

 C = repelem(gpcf.V(a), diffnb)';
 end

function reccf = gpcf_covar_recappend(reccf, ri, gpcf)
%  RECAPPEND  Record append
%
%  Description:
%   · RECCF = GPCF_COVAR_RECAPPEND(RECCF, RI, GPCF) takes a
%     correlation structure record structure RECCF, record index RI
%     and correlation structure GPCF with the current MCMC
%     samples of the parameters. Returns RECCF which contains
%     all the old samples and the current samples from GPCF.
%     This subfunction is needed when using MCMC sampling (gp_mc).
%
%  * See also:
%    GP_MC and GP_MC -> RECAPPEND

  if nargin == 2
      % initialize the record
      reccf.type = 'gpcf_covar';

      % initialize parameters for correlations functions
      for i = 1:ri.numberClass
          reccf.corrFun{i} = ri.corrFun{i}.fh.recappend([], ri.corrFun{i});
      end

      % Initialize parameters
      reccf.R = [];
      reccf.V = [];

      % Set the function handles
      reccf.fh.realtoz = @gpcf_covar_realtoz;
      reccf.fh.ztoreal = @gpcf_covar_ztoreal;
      reccf.fh.rhotoreal = @gpcf_covar_rhotoreal;
      reccf.fh.realtorho = @gpcf_covar_realtorho;
      reccf.fh.pak = @gpcf_covar_pak;
      reccf.fh.unpak = @gpcf_covar_unpak;
      reccf.fh.lp = @gpcf_covar_lp;
      reccf.fh.lpg = @gpcf_covar_lpg;
      reccf.fh.cfg = @gpcf_covar_cfg;
      reccf.fh.cov = @gpcf_covar_cov;
      reccf.fh.sigma = @gpcf_covar_sigma;
      reccf.fh.trcov = @gpcf_covar_trcov;
      reccf.fh.trvar = @gpcf_covar_trvar;
      reccf.fh.recappend = @gpcf_covar_recappend;
      reccf.p = [];
      reccf.p.R = [];
      reccf.p.V = [];

      if isfield(ri.p, 'R') && ~isempty(ri.p.R)
          reccf.p.R = ri.p.R;
      end

      if isfield(ri.p, 'V') && ~isempty(ri.p.V)
          for i = 1:ri.numberClass
              reccf.p.V.(ri.varfields{i}).p = ri.p.V.(ri.varfields{i}).p;
          end
      end

      if isfield(ri, 'classVariables')
          reccf.classVariables = ri.classVariables;
          reccf.numberClass = ri.numberClass;
          reccf.vectSize = ri.vectSize;
          reccf.aValue = ri.aValue;
          reccf.varfields = ri.varfields;
          reccf.index = ri.index;
      end

  else
      % Append to the record
      gpp = gpcf.p;

      if ~ isfield(gpcf,'metric')
          % record correlation function elements
          for i = 1:gpcf.numberClass
              cf = gpcf.corrFun{i};
              reccf.corrFun{i} = cf.fh.recappend(reccf.corrFun{i}, ri, cf);
          end

          % record R elements
          reccf.R(ri, :) = gpcf.R;
          if isfield(gpp, 'R') && ~isempty(gpp.R)
              reccf.p.R = gpp.R.fh.recappend(reccf.p.R, ri, gpcf.p.R);
          end

          % record V elements
          reccf.V(ri, :) = gpcf.V;
          if isfield(gpp, 'V') && ~isempty(gpp.V)
              for i = 1:gpcf.numberClass
                  cfvf = gpcf.varfields{i};
                  reccf.p.V.(cfvf).p = ...
                      gpp.V.(cfvf).p.fh.recappend(reccf.p.V.(cfvf).p, ...
                      ri, gpcf.p.V.(cfvf).p);
              end
          end
      end

  end
end


function M = fixedrankfactory_2factors_preconditioned(m, n, k)
% Manifold of m-by-n matrices of rank k with two factor quotient geometry.
%
% function M = fixedrankfactory_2factors_preconditioned(m, n, k)
%
% This geometry is tuned to least-squares problems such as low-rank matrix
% completion with ell-2 loss.
%
% A point X on the manifold is represented as a structure with two
% fields: L and R. The matrices L (m-by-k) and R (n-by-k) are
% full column-rank matrices such that X = L*R'.
%
% Tangent vectors are represented as a structure with two fields: L, R.
%
% Please cite the Manopt paper as well as the research paper:
%     @Techreport{mishra2012optimized,
%       Title   = {A {R}iemannian geometry for low-rank matrix completion},
%       Author  = {Mishra, B. and Adithya Apuroop, K. and Sepulchre, R.},
%       Journal = {Arxiv preprint arXiv:1211.1550},
%       Year    = {2012}
%     }
%
%
% See also: fixedrankembeddedfactory fixedrankfactory_2factors fixedrankfactory_3factors_preconditioned

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, Dec. 30, 2012.
% Contributors:
% Change log:
%
%	April 04, 2015 (BM):
%      Cosmetic changes including avoiding storing the inverse of a
%       k-by-k matrix.

    M.name = @() sprintf('LR''(tuned to least square problems) quotient manifold of %dx%d matrices of rank %d', m, n, k);

    M.dim = @() (m+n-k)*k;

    % Some precomputations at the point X to be used in the inner product
    % (and pretty much everywhere else).
    function X = prepare(X)
        if ~all(isfield(X,{'LtL','RtR'}))
            L = X.L;
            R = X.R;
            X.LtL = L'*L;
            X.RtR = R'*R;
        end
    end

    % The choice of metric is motivated by symmetry and
    % tuned to least-squares cost function.
    M.inner = @iproduct;
    function ip = iproduct(X, eta, zeta)
        X = prepare(X);
        ip = trace(X.RtR*(eta.L'*zeta.L)) + trace(X.LtL*(eta.R'*zeta.R)); % Scaled metric
    end

    M.norm = @(X, eta) sqrt(M.inner(X, eta, eta));

    M.dist = @(x, y) error('fixedrankfactory_2factors_preconditioned.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.egrad2rgrad = @egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)
        X = prepare(X);

        % Riemannian gradient
        rgrad.L = egrad.L/X.RtR;
        rgrad.R = egrad.R/X.LtL;
    end

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(X, egrad, ehess, eta)
        X = prepare(X);

        % Riemannian gradient.
        rgrad = egrad2rgrad(X, egrad);

        % Directional derivative of the Riemannian gradient.
        Hess.L = ehess.L/X.RtR - 2*egrad.L*(X.RtR \ (symm(eta.R'*X.R) / X.RtR));
        Hess.R = ehess.R/X.LtL - 2*egrad.R*(X.LtL \ (symm(eta.L'*X.L) / X.LtL));

        % We still need a correction factor for the non-constant metric.
        Hess.L = Hess.L + rgrad.L*(symm(eta.R'*X.R)/X.RtR) + eta.L*(symm(rgrad.R'*X.R)/X.RtR) - X.L*(symm(eta.R'*rgrad.R)/X.RtR);
        Hess.R = Hess.R + rgrad.R*(symm(eta.L'*X.L)/X.LtL) + eta.R*(symm(rgrad.L'*X.L)/X.LtL) - X.R*(symm(eta.L'*rgrad.L)/X.LtL);

        % Project on the horizontal space.
        Hess = M.proj(X, Hess);
    end

    M.proj = @projection;
    function etaproj = projection(X, eta)
        X = prepare(X);

        % Projection onto the horizontal space.
        Lambda = 0.5*((eta.R'*X.R)/X.RtR  -   X.LtL\(X.L'*eta.L));
        etaproj.L = eta.L + X.L*Lambda;
        etaproj.R = eta.R - X.R*Lambda';
    end

    M.tangent = M.proj;

    M.tangent2ambient = @(X, eta) eta;

    M.retr = @retraction;
    function Y = retraction(X, eta, t)
        if nargin < 3
            t = 1.0;
        end
        Y.L = X.L + t*eta.L;
        Y.R = X.R + t*eta.R;

        % Numerical conditioning step: a simpler version.
        % We need to ensure that L and R are do not have very relative
        % skewed norms.

        scaling = norm(X.L, 'fro')/norm(X.R, 'fro');
        scaling = sqrt(scaling);
        Y.L = Y.L / scaling;
        Y.R = Y.R * scaling;

        % These are reused in the computations of gradient and Hessian.
        Y = prepare(Y);
    end

    M.exp = @exponential;
    function Y = exponential(X, eta, t)
        if nargin < 3
            t = 1.0;
        end

        Y = retraction(X, eta, t);
        warning('manopt:fixedrankfactory_2factors_preconditioned:exp', ...
            ['Exponential for fixed rank ' ...
            'manifold not implemented yet. Used retraction instead.']);
    end

    M.hash = @(X) ['z' hashmd5([X.L(:) ; X.R(:)])];

    M.rand = @random;

    function X = random()
        X.L = randn(m, k);
        X.R = randn(n, k);
    end

    M.randvec = @randomvec;
    function eta = randomvec(X)
        eta.L = randn(m, k);
        eta.R = randn(n, k);
        eta = projection(X, eta);
        nrm = M.norm(X, eta);
        eta.L = eta.L / nrm;
        eta.R = eta.R / nrm;
    end

    M.lincomb = @lincomb;

    M.zerovec = @(X) struct('L', zeros(m, k),'R', zeros(n, k));

    M.transp = @(x1, x2, d) projection(x2, d);

    % vec and mat are not isometries, because of the scaled inner metric.
    M.vec = @(X, U) [U.L(:) ; U.R(:)];

    M.mat = @(X, u) struct('L', reshape(u(1:(m*k)), m, k), ...
        'R', reshape(u((m*k+1):end), n, k));

    M.vecmatareisometries = @() false;

    % Auxiliary functions
    symm = @(M) .5*(M+M');
end

% Linear combination of tangent vectors.
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d.L = a1*d1.L;
        d.R = a1*d1.R;
    elseif nargin == 5
        d.L = a1*d1.L + a2*d2.L;
        d.R = a1*d1.R + a2*d2.R;
    else
        error('Bad use of fixedrankfactory_2factors_preconditioned.lincomb.');
    end

end


function y = loadPcwConst(type, nSamples)
%LOADPCWCONST Creates a piecewise constant signal or image

% written by M. Storath
% $Date: 2014-09-02 14:56:23 +0200 (Di, 02. Sep 2014) $	$Revision: 105 $

if not(exist('nSamples', 'var'))
    nSamples = 2^8;
end

x = linspace(-1, 1, nSamples)';
y = zeros(size(x));

switch type
    case 'rect'
        y = (-0.5 < x) & ( x < 0.5);
    case {'step', 'heaviside'}
        y = x >= 0;
    case 'jumps'
        % two jumps
        y = (-0.3 < x) & ( x < -0.1) + 2 * ((0.1 < x) & ( x < 0.4));
    case 'equidistant'
        n = 8; % number of partitions
        step = nSamples / n;
        heights = [3, 1, 7, 6, 5, 0,6,4];
        for i = 1:n
            iv = ((i-1) * step + 1) : (i * step);
            y(iv) = heights(i) / max(heights);
        end

    case 'sampleDec'
        heights = [3, 1, 7, 6, 5, 0,6,6];
        jumps = diff(heights);
        jumpDist = [30, 40, 32, 32, 32, 50, 60];
        jumpIdx = cumsum(jumpDist);
        y(jumpIdx) = jumps;
        y = cumsum(y);
        y = mat2gray(y);

    case 'sampleDec2'
        heights = [3, 1, 7, 6, 5, 0,6,6];
        jumps = diff(heights);
        jumpDist = [30, 40, 32, 32, 32, 50, 40];
        jumpIdx = cumsum(jumpDist);
        y(jumpIdx) = jumps;
        y = cumsum(y);
        y = mat2gray(y);

    case 'equidistant2'
        n = 8; % number of partitions
        step = nSamples / n;
        heights = [3, 1, 7, 6, 5, 4,6,4];
        for i = 1:n
            iv = ((i-1) * step + 1) : (i * step);
            y(iv) = heights(i) / max(heights);
        end

    case 'sample1'
        steps = [2, 4, 3, 3, 2, 5, 3, 3, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 5.5, 4.5, 3.5,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

    case 'sample2'
        steps = [2, 4, 3, 3, 2, 5, 2, 3, 3,6,4,2,8,3,4, 4]; %15
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 6, 5, 4,6,0, 2, 4, 3, 1.5, 7, 9, 2];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

   case 'sample3'
        steps = [2, 4, 3, 6, 4, 5, 4, 3, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 1, 7, 6, 5, 4,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

   case 'sample4'
        %steps = [3, 2, 5, 10, 4, 3, 3];
        %steps = [ -Inf, (cumsum(steps ./ sum(steps)) - 0.5) * 2];
        %heights = [4, 2,3, 3.1, 3.05, 1, 1];
        steps = [2, 4, 2, 2, 1, 1];
        steps = [ -Inf, (cumsum(steps ./ sum(steps)) - 0.5) * 2];
        heights = [0.0, 0.1, 0.05, 1, 0.5, 0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(abs(heights));
        end

   case 'sample5'
        steps = [3, 5, 3, 4, 6, 4, 3, 5, 3];
        steps = (cumsum(steps ./ sum(steps)) - 0.5) * 2;
        heights = [3, 7, 2, 4, 5, 2,6,0];
        n= numel(heights);
        for i = 1:n
            idx = find((steps(i) < x) & (x <= steps(i+1)));
            y( idx ) = heights(i) / max(heights);
        end

    case 'geo1'
        y = double(imread('geo1.png'))/255;

    case 'geo2'
        y = double(imread('geo2.png'))/255;

    case 'geo3'
        y = double(imread('geo3.png'))/255;

    case 'geo4'
        y = double(imread('geo4.png'))/255;

    case 'geo5'
        y = double(imread('geo5.png'))/255;

    case 'geo6'
        y = double(imread('geo6.png'))/255;

    case 'geo7'
        y = double(imread('geo7.png'))/255;

    case 'geo8'
        y = double(imread('geo8.png'))/255;

    case 'overlay'
        I1 = double(imread('rectangle.png'));
        I2 = double(imread('octo.png'));
        y =  (I1 + 2*I2)/ (3*255);

    otherwise
        error('This option does not exist.')
end

% cast to double
y = double(y);

end


function [ r, lchar, ierror ] = s_to_r8 ( s )

%*****************************************************************************80
%
%% S_TO_R8 reads an R8 from a string.
%
%  Discussion:
%
%    This routine will read as many characters as possible until it reaches
%    the end of the string, or encounters a character which cannot be
%    part of the real number.
%
%    Legal input is:
%
%       1 blanks,
%       2 '+' or '-' sign,
%       2.5 spaces
%       3 integer part,
%       4 decimal point,
%       5 fraction part,
%       6 'E' or 'e' or 'D' or 'd', exponent marker,
%       7 exponent sign,
%       8 exponent integer part,
%       9 exponent decimal point,
%      10 exponent fraction part,
%      11 blanks,
%      12 final comma or semicolon.
%
%    with most quantities optional.
%
%  Example:
%
%    S                 R
%
%    '1'               1.0
%    '     1   '       1.0
%    '1A'              1.0
%    '12,34,56'        12.0
%    '  34 7'          34.0
%    '-1E2ABCD'        -100.0
%    '-1X2ABCD'        -1.0
%    ' 2E-1'           0.2
%    '23.45'           23.45
%    '-4.2E+2'         -420.0
%    '17d2'            1700.0
%    '-14e-2'         -0.14
%    'e2'              100.0
%    '-12.73e-9.23'   -12.73 * 10.0**(-9.23)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string containing the
%    data to be read.  Reading will begin at position 1 and
%    terminate at the end of the string, or when no more
%    characters can be read to form a legal real.  Blanks,
%    commas, or other nonnumeric data will, in particular,
%    cause the conversion to halt.
%
%    Output, real R, the value that was read from the string.
%
%    Output, integer LCHAR, the number of characters of S that were used to form R.
%
%    Output, integer IERROR, is 0 if no error occurred.
%
  s_length = s_len_trim ( s );
  ierror = 0;
  r = 0.0;
  lchar = -1;
  isgn = 1;
  rtop = 0.0;
  rbot = 1.0;
  jsgn = 1;
  jtop = 0;
  jbot = 1;
  ihave = 1;
  iterm = 0;

  while ( 1 )

    lchar = lchar + 1;
    c = s(lchar+1);
%
%  Blank character.
%
    if ( c == ' ' )

      if ( ihave == 2 )

      elseif ( ihave == 6 | ihave == 7 )
        iterm = 1;
      elseif ( 1 < ihave )
        ihave = 11;
      end
%
%  Comma.
%
    elseif ( c == ',' | c == ';' )

      if ( ihave ~= 1 )
        iterm = 1;
        ihave = 12;
        lchar = lchar + 1;
      end
%
%  Minus sign.
%
    elseif ( c == '-' )

      if ( ihave == 1 );
        ihave = 2;
        isgn = -1;
      elseif ( ihave == 6 )
        ihave = 7;
        jsgn = -1;
      else
        iterm = 1;
      end
%
%  Plus sign.
%
    elseif ( c == '+' )

      if ( ihave == 1 )
        ihave = 2;
      elseif ( ihave == 6 )
        ihave = 7;
      else
        iterm = 1;
      end
%
%  Decimal point.
%
    elseif ( c == '.' )

      if ( ihave < 4 )
        ihave = 4;
      elseif ( 6 <= ihave & ihave <= 8 )
        ihave = 9;
      else
        iterm = 1;
      end
%
%  Exponent marker.
%
    elseif ( ch_eqi ( c, 'E' ) | ch_eqi ( c, 'D' ) )

      if ( ihave < 6 )
        ihave = 6;
      else
        iterm = 1;
      end
%
%  Digit.
%
    elseif ( ihave < 11 & ch_is_digit ( c ) )

      if ( ihave <= 2 )
        ihave = 3;
      elseif ( ihave == 4 )
        ihave = 5;
      elseif ( ihave == 6 | ihave == 7 )
        ihave = 8;
      elseif ( ihave == 9 )
        ihave = 10;
      end

      d = ch_to_digit ( c );

      if ( ihave == 3 )
        rtop = 10.0 * rtop + d;
      elseif ( ihave == 5 )
        rtop = 10.0 * rtop + d;
        rbot = 10.0 * rbot;
      elseif ( ihave == 8 )
        jtop = 10 * jtop + d;
      elseif ( ihave == 10 )
        jtop = 10 * jtop + d;
        jbot = 10 * jbot;
      end
%
%  Anything else is regarded as a terminator.
%
    else
      iterm = 1;
    end
%
%  If we haven't seen a terminator, and we haven't examined the
%  entire string, go get the next character.
%
    if ( iterm == 1 | s_length <= lchar + 1 )
      break;
    end

  end
%
%  If we haven't seen a terminator, and we have examined the
%  entire string, then we're done, and LCHAR is equal to S_LENGTH.
%
  if ( iterm ~= 1 & lchar + 1 == s_length )
    lchar = s_length;
  end
%
%  Number seems to have terminated.  Have we got a legal number?
%  Not if we terminated in states 1, 2, 6 or 7!
%
  if ( ihave == 1 | ihave == 2 | ihave == 6 | ihave == 7 )
    ierror = ihave;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'S_TO_R8 - Fatal error!\n' );
    fprintf ( 1, '  IHAVE = %d\n', ihave );
    error ( 'S_TO_R8 - Fatal error!' );
  end
%
%  Number seems OK.  Form it.
%
  if ( jtop == 0 )
    rexp = 1.0;
  else

    if ( jbot == 1 )
      rexp = 10.0^( jsgn * jtop );
    else
      rexp = jsgn * jtop;
      rexp = rexp / jbot;
      rexp = 10.0^rexp;
    end

  end

  r = isgn * rexp * rtop / rbot;

  return
end


function g = gaussianKernGradient(kern, x, varargin)

% GAUSSIANKERNGRADIENT Gradient of gaussian kernel's parameters.
% FORMAT
% DESC computes the gradient of
%	functions with respect to the gaussian kernel's
%	parameters. As well as the kernel structure and the input positions,
%	the user provides a matrix PARTIAL which gives the partial
%	derivatives of the function with respect to the relevant elements of
%	the kernel matrix.
% RETURN g:  gradients of the function of interest with respect to the
%	   kernel parameters. The ordering of the vector should match that
%	   provided by the function kernExtractParam.
% ARG kern : the kernel structure for which the gradients are being
%	   computed.
% ARG x : the input locations for which the gradients are being
%	   computed.
% ARG partial : matrix of partial derivatives of the function of
%	   interest with respect to the kernel matrix. The argument takes the
%	   form of a square matrix of dimension  numData, where numData is
%	   the number of rows in X.
%
% FORMAT
% DESC  computes the derivatives
%	as above, but input locations are now provided in two matrices
%	associated with rows and columns of the kernel matrix.
% RETURN g : gradients of the function of interest with respect to the
%	   kernel parameters.
% ARG kern : the kernel structure for which the gradients are being
%	   computed.
% ARG x1 : the input locations associated with the rows of the kernel
%	   matrix.
% ARG x2 : the input locations associated with the columns of the kernel
%	   matrix.
% ARG partial : matrix of partial derivatives of the function of
%	   interest with respect to the kernel matrix. The matrix should have
%	   the same number of rows as X1 and the same number of columns as X2
%	   has rows.
%
% SEEALSO : gaussianKernParamInit, kernGradient,
% gaussianKernDiagGradient, kernGradX
%
% COPYRIGHT : Mauricio A. Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS : Mauricio A. Alvarez, 2009.

% KERN

if nargin < 4
    x2 = x;
    covPar = varargin{1};
else
    x2 = varargin{1};
    covPar = varargin{2};
end

if kern.isArd
    [K, Kbase] = gaussianKernCompute(kern, x, x2);
    matGrad = zeros(kern.inputDimension,1);
    temp = 0.5*covPar.*K;
    for i = 1:kern.inputDimension,
        pX = x(:,i);
        X = pX(:, ones(1, size(x2,1)));
        pX2 = x2(:,i)';
        X2 = pX2(ones(size(x,1),1), :);
%         X = repmat(x(:,i),1, size(x2,1));
%         X2 = repmat(x2(:,i)',size(x,1),1);
        X_X2 = X - X2;
        matGrad(i) = -sum(sum(temp.*X_X2.*X_X2));
    end
else
    [K, Kbase, n2] = gaussianKernCompute(kern, x, x2);
    matGrad = - 0.5*sum(sum(covPar.*K.*n2,2));

end
g = [matGrad(:)' sum(sum(covPar.*Kbase))];


function [D,DJacob,DHess,pDpt]=DPolarLin2D(x,q0Dir,param4)
%%DPOLARCV2D The diffusion matrix for a continuous-time constant heading
%            motion model where the target state is given in terms of
%            position, direction of velocity in radians counterclockwise
%            from the x-axis, and speed.
%
%INPUTS: x The 4XN set of target state vectors in 2D space in the order of
%          [2D position;direction angle;speed] for each column where the
%          heading angle is measured in radians counterclockwise from the
%          x-axis. Alternatively, a 5XN state vector of
%          [2D position;direction angle;speed; speed derivative] can be
%          used to cover a linearlly accelerating target. Only the
%          dimensionality of x matters; the components are ignored. The
%          dimensionality is used to determine whether linear acceleration
%          is modeled and what N is.
%    q0Dir The power spectral density of the process noise for the heading
%          direction. The units are rad^2/s.
%   param4 If x is only 4D, that is, does not model a constant linear
%          acceleration, then param4 is the power spectral density of the
%          process noise for the speed. The units are m^2/s^3. Otherwise,
%          param4 is the power spectral density of the noise for the
%          linear acceleration and its units are m^2/s^5.
%
%OUTPUT: D The 4X2XN set of N diffusion matrices of a continuous-time
%          linear additive noise model where the velocity is given in terms
%          of direction and speed and the noise is only added to the
%          direction and speed components or the 5X2XN set of matrices
%          where noise is added to the direction and acceleration
%          components if a linear acceleration is assumed.
%   DJacob, DHess The xDimX2XxDim, xDimX2XxDimXxDim matrices of first and
%           second partial derivatives of the elements of D with respect to
%           x. These are all zero, since D is a constant. it is the same
%           for all D and is not repeated N times.
%      pDpt The xDimX2 partial derivative of D with respect to time. This
%           is all zeros, because D is a constant.
%
%Ideas for setting q0Dir  can be obtained using processNoiseSuggest with
%'PolyKal-ROT' and order 0. Ideas for setting param4 can
%be obtained using processNoiseSuggest with 'PolyKal-ROT' and order 1 or 2
%respectively depending on whether x is constant velocity or constant
%acceleration.
%
%The idea of decomposing the velocity into direction and speed components
%is presented in [1], where the direction component is differently defined
%and additional derivatives are used.
%
%This diffusion matrix goes with the drift function aPolarLin2D.
%
%REFERENCES:
%[1] J. L. Gertz, "Multisensor surveillance for improved aircraft
%    tracking," The Lincoln Laboratory Journal, vol. 2, no. 3, pp. 381-396,
%    1989.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(x,2);

q0DirR=sqrt(q0Dir);
param4R=sqrt(param4);

numDim=size(x,1);

if(numDim==4)%If it is a constant velocity model
    D=repmat([0,      0;
                0,      0;
                q0DirR, 0;
                0,     param4R],[1,1,N]);
elseif(numDim==5)%If it is a constant acceleration model.
    D=repmat([0,      0;
                0,      0;
                q0DirR, 0;
                0,      0;
                0,      param4R],[1,1,N]);
else
    error('The length of x is neither 4 nor 5.');
end

if(nargout>1)
    DJacob=zeros(numDim,2,numDim);
    if(nargout>2)
        DHess=zeros(numDim,2,numDim,numDim);
        if(nargout>3)
            pDpt=zeros(numDim,2);
        end
    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 vol=smoothbinvol(vol,layer)
%
% vol=smoothbinvol(vol,layer)
%
% perform a memory-limited 3D image smoothing
%
% author: Qianqian Fang <q.fang at neu.edu>
%
% input:
%     vol: a 3D volumetric image to be smoothed
%     layer: number of iterations for the smoothing
%
% output:
%     vol: the volumetric image after smoothing
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

dim=size(vol);
dxy=dim(1)*dim(2);
fulllen=prod(dim);

weight=1./6.;
step=4000;

% in case vol is a logical
vol=double(vol);
offs=[1,-1,dim(1), -dim(1),dxy, -dxy];

for i=1:layer
  % find all non-zero values
  idx=find(vol);
  % get the neighbors of all the non-zero values
  % this may cause wrapping -- TODO
  val=vol(idx);
  for k=1:6
    nextidx=idx+offs(k);
    % find all 1-valued voxels that are located within the domain
	  goodidx=find(nextidx>0 & nextidx<fulllen);
    % for all neighboring voxels, add a fraction from the non-0 voxels
    % problematic when running in parallel (racing)
    len=length(goodidx);
    % control granualarity with step
    if(len>step)
        for j=1:step:len-step
            vol(nextidx(goodidx(j:j+step-1)))=vol(nextidx(goodidx(j:j+step-1)))+weight*val(goodidx(j:j+step-1));
        end
        vol(nextidx(goodidx(j+step:end)))=vol(nextidx(goodidx(j+step:end)))+weight*val(goodidx(j+step:end));
    else
        vol(nextidx(goodidx))=vol(nextidx(goodidx))+weight*val(goodidx);
    end
    % the above line may change the values of the non-zero voxels, recover
    % them
  end
  vol(idx)=val;
end


function rrd = jplephem (et, ntarg, ncent)

% reads the jpl planetary ephemeris and gives
% the position and velocity of the point 'ntarg'
% with respect to point 'ncent'

% input

%   et    = julian ephemeris date at which interpolation is wanted

%   ntarg = integer number of 'target' point

%   ncent = integer number of center point

%   the numbering convention for 'ntarg' and 'ncent' is:

%        1 = mercury           8 = neptune
%        2 = venus             9 = pluto
%        3 = earth            10 = moon
%        4 = mars             11 = sun
%        5 = jupiter          12 = solar-system barycenter
%        6 = saturn           13 = earth-moon barycenter
%        7 = uranus           14 = nutations (longitude and obliq)
%                             15 = librations, if on ephemeris file

%        if nutations are wanted, set ntarg = 14.
%        for librations, set ntarg = 15. set ncent = 0.

% output

%   rrd = output 6-word array containing position and velocity
%         of point 'ntarg' relative to 'ncent'. the units are au and
%         au/day. for librations the units are radians and radians
%         per day. in the case of nutations the first four words of
%         rrd will be set to nutations and rates, having units of
%         radians and radians/day.

%         the option is available to have the units in km and km/sec.
%         for this, set km = 1 via global in the calling program.

% Orbital Mechanics with MATLAB

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

global cval ss au emrat ncon ipt np nv twot pc vc

global iephem ephname bary pvsun nrl fid lpt

rrd = zeros(6, 1);

list = zeros(12, 1);

et2 = zeros(2, 1);

% load time array

et2(1) = et;

et2(2) = 0;

% first entry?

if (iephem == 1)

    pvsun = zeros(6, 1);

    % read header file data

    fid = fopen(ephname, 'r');

    ttl = fread(fid, 252);

    cnam = fread(fid, 2400);

    ss = fread(fid, 3, 'double');

    ncon = fread(fid, 1, 'int');

    % astronomical unit

    au = fread(fid, 1, 'double');

    % earth-moon ratio

    emrat = fread(fid, 1, 'double');

    ipt = fread(fid, [3 12], 'int');

    numde = fread(fid, 1, 'int');

    lpt = fread(fid, 3, 'int');

    % move to next record

    status = fseek(fid, 8144, 'bof');

    % read "constant" values

    cval = fread(fid, 400, 'double');

    % initialization

    nrl = 0;

    bary = 0;

    pc(1) = 1;
    pc(2) = 0;

    vc(2) = 1;

    np = 2;
    nv = 3;

    twot = 0;

    iephem = 0;
end

if (ntarg == ncent)

    return;

end

%%%%%%%%%%%%%%%%%%%%%%
% nutations requested?
%%%%%%%%%%%%%%%%%%%%%%

if (ntarg == 14)

    if (ipt(2, 12) > 0)

        list(11) = 2;

        [pv, rrd] = state(et2, list);

        list(11) = 0;

        return;

    else

        fprintf('\n\njplephem - no nutations on this ephemeris file \n');

        return;

    end
end

%%%%%%%%%%%%%%%%%%%%%%%
% librations requested?
%%%%%%%%%%%%%%%%%%%%%%%

if (ntarg == 15)

    if (lpt(2) > 0)

        list(12) = 2;

        [pv, rrd] = state(et2, list);

        list(12) = 0;

        for i = 1:1:6
            rrd(i) = pv(i, 11);
        end

        return

    else

        fprintf('\n\n no librations on this ephemeris file \n');

        return;

    end

end

% force barycentric output by function 'state'

bsave = bary;

bary = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set up proper entries in 'list' array for state call
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:2

    k = ntarg;

    if (i == 2)
        k = ncent;
    end

    if (k <= 10)
        list(k) = 2;
    end

    if (k == 10)
        list(3) = 2;
    end

    if (k == 3)
        list(10) = 2;
    end

    if (k == 13)
        list(3) = 2;
    end

end

%%%%%%%%%%%%%%%%%%%%
% make call to state
%%%%%%%%%%%%%%%%%%%%

[pv, rrd] = state(et2, list);

if (ntarg == 11 || ncent == 11)
    for i = 1:1:6
        pv(i, 11) = pvsun(i);
    end
end

if (ntarg == 12 || ncent == 12)
    for i = 1:1:6
        pv(i, 12) = 0;
    end
end

if (ntarg == 13 || ncent == 13)
    for i = 1:1:6
        pv(i, 13) = pv(i, 3);
    end
end

if (ntarg * ncent == 30 && ntarg + ncent == 13)
    for i = 1:1:6
        pv(i, 3) = 0;
    end
else
    if (list(3) == 2)
        for i = 1:1:6
            pv(i, 3) = pv(i, 3) - pv(i, 10) / (1 + emrat);
        end
    end

    if (list(10) == 2)
        for i = 1:1:6
            pv(i, 10) = pv(i, 3) + pv(i, 10);
        end
    end
end

for i = 1:1:6
    rrd(i) = pv(i, ntarg) - pv(i, ncent);
end

bary = bsave;

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

function [pv, nut] = state(et2, list)

% reads and interpolates the jpl planetary ephemeris file

% input

%   et2    2-word julian ephemeris epoch at which interpolation
%          is wanted.  any combination of et2(1) + et2(2) which falls
%          within the time span on the file is a permissible epoch.

%          a. for ease in programming, the user may put the
%             entire epoch in et2(1) and set et2(2) = 0.

%          b. for maximum interpolation accuracy, set et2(1) equal
%             to the most recent midnight at or before interpolation
%             epoch and set et2(2) equal to fractional part of a day
%             elapsed between et2(1) and epoch.

%          c. as an alternative, it may prove convenient to set
%             et2(1) = some fixed epoch, such as start of integration,
%             and et2(2) = elapsed interval between and epoch.

%   list   12-word integer array specifying what interpolation
%          is wanted for each of the bodies on the file.
%
%          list(i) = 0 => no interpolation for body i
%                  = 1 => position only
%                  = 2 => position and velocity

%          the designation of the astronomical bodies by i is:

%              i =  1 => mercury
%                =  2 => venus
%                =  3 => earth-moon barycenter
%                =  4 => mars
%                =  5 => jupiter
%                =  6 => saturn
%                =  7 => uranus
%                =  8 => neptune
%                =  9 => pluto
%                = 10 => geocentric moon
%                = 11 => nutations in longitude and obliquity
%                = 12 => lunar librations (if on file)

% output

%   pv   6 x 11 array that will contain requested interpolated
%        quantities. the body specified by list(i) will have its
%        state in the array starting at pv(1, i). (on any given
%        call, only those words in 'pv' which are affected by the
%        first 10 'list' entries (and by list(12) if librations are
%        on the file) are set. the rest of the 'pv' array
%        is untouched).  the order of components starting in
%        pv(1, i) is x, y, z, dx, dy, dz.

%        all output vectors are referenced to the earth mean
%        equator and equinox of j2000 if the de number is 200 or
%        greater; of b1950 if the de number is less than 200.

%        the moon state is always geocentric; the other nine states
%        are either heliocentric or solar-system barycentric,
%        depending on the setting of common flags (see below).

%        lunar librations, if on file, are put into pv(k, 11) if
%        list(12) is 1 or 2.
%
%   nut  4-word array that will contain nutations and rates,
%        depending on the setting of list(11). the order of
%        quantities in nut is:

%        d psi  (nutation in longitude)
%        d epsilon (nutation in obliquity)
%        d psi dot
%        d epsilon dot

% global

%   km    logical flag defining physical units of the output states
%         = 1 => kilometers and kilometers/second
%         = 0 => au and au/day

%         default value = 0 (km determines time unit
%         for nutations and librations. angle unit is always radians.)

%   bary  logical flag defining output center.
%         only the 9 planets are affected.
%         bary = 1 => center is solar-system barycenter
%              = 0 => center is sun
%         default value = 0

%   pvsun 6-word array containing the barycentric position and
%         velocity of the sun

global ss au ipt lpt

global nrl fid km bary pvsun coef

nut = zeros(4, 1);

pv = zeros(6, 11);

if (et2(1) == 0)
    return
end

s = et2(1) - 0.5;

tmp = split(s);

pjd(1) = tmp(1);

pjd(2) = tmp(2);

tmp = split(et2(2));

pjd(3) = tmp(1);

pjd(4) = tmp(2);

pjd(1) = pjd(1) + pjd(3) + 0.5;

pjd(2) = pjd(2) + pjd(4);

tmp = split(pjd(2));

pjd(3) = tmp(1);

pjd(4) = tmp(2);

pjd(1) = pjd(1) + pjd(3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% error return for epoch out of range
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (pjd(1) + pjd(4) < ss(1) || pjd(1) + pjd(4) > ss(2))
    fprintf('\n\n error in state - epoch out of range \n');

    return;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate record number and relative time in interval
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nr = fix((pjd(1) - ss(1)) / ss(3)) + 2;

if (pjd(1) == ss(2))
    nr = nr - 1;
end

t(1) = ((pjd(1) - ((nr-2) * ss(3) + ss(1))) + pjd(4)) / ss(3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read correct record if not in core
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (nr ~= nrl)
    nrl = nr;

    status = fseek(fid, nr * 8144, 'bof');

    coef = fread(fid, 1018, 'double');
end

if (km == 1)
    t(2) = ss(3) * 86400;
    aufac = 1;
else
    t(2) = ss(3);
    aufac = 1 / au;
end

% interpolate barycentric state vector of sun

tmpv = zeros(3, 2);

ibuf = ipt(1, 11);
ncf = ipt(2, 11);
na = ipt(3, 11);

tmpv = interp(ibuf, t, ncf, 3, na, 2);

k = 0;

for j = 1:1:2
    for i = 1:1:3
        k = k + 1;
        pvsun(k) = tmpv(i, j) * aufac;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check and interpolate bodies requested
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tmpv = zeros(3, 2);

tempv = zeros(6, 1);

for i = 1:1:10
    if (list(i) ~= 0)

        ibuf = ipt(1, i);
        ncf = ipt(2, i);
        na = ipt(3, i);

        tmpv = interp(ibuf, t, ncf, 3, na, list(i));

        k = 0;

        for j = 1:1:2
            for m = 1:1:3
                k = k + 1;
                tempv(k) = tmpv(m, j);
            end
        end

        for j = 1:1:6
            if (i <= 9 && bary == 0)
                pv(j, i) = tempv(j) * aufac - pvsun(j);
            else
                pv(j, i) = tempv(j) * aufac;
            end
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do nutations if requested (and if on file)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (list(11) > 0 && ipt(2, 12) > 0)

    ibuf = ipt(1, 12);
    ncf = ipt(2, 12);
    na = ipt(3, 12);

    nut = interp(ibuf, t, ncf, 2, na, list(11));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get librations if requested (and if on file)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (list(12) > 0 && lpt(2) > 0)
    tmpv = interp(lpt(1), t, lpt(2), 3, lpt(3), list(12));

    for i = 1:1:3
        pv(i, 11) = tmpv(i, 1);

        pv(i + 3, 11) = tmpv(i, 2);
    end
end

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

function pv = interp(ibuf, t, ncf, ncm, na, ifl)

% this function differentiates and interpolates a set
% of chebyshev coefficients to give position and velocity

% input

%   buf   1st location of array of chebyshev coefficients of position

%   t     t(1) is fractional time in interval covered by
%         coefficients at which interpolation is wanted
%         (0 <= t(1) <= 1). t(2) is length of whole
%         interval in input time units.

%   ncf   number of coefficients per component

%   ncm   number of components per set of coefficients

%   na    number of sets of coefficients in full array
%         (i.e., number of sub-intervals in full interval)

%   ifl   integer flag
%         = 1 for positions only
%         = 2 for pos and vel

% output

%   pv    interpolated quantities requested.  dimension
%         expected is pv(ncm, ifl)

global coef np nv twot pc vc

pv = zeros(6, 12);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get correct sub-interval number for this set of
% coefficients and get normalized chebyshev time
% within that subinterval.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dna = na;

dt1 = fix(t(1));

temp = dna * t(1);

ll = fix(temp - dt1) + 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% tc is the normalized chebyshev time (-1 <= tc <= 1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tc = 2 * (mod(temp, 1) + dt1) - 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check to see whether chebyshev time has changed,
% and compute new polynomial values if it has.
% (the element pc(2) is the value of t1(tc) and hence
% contains the value of tc on the previous call)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (tc ~= pc(2))
    np = 2;
    nv = 3;
    pc(2) = tc;
    twot = tc + tc;
end

% be sure that at least 'ncf' polynomials have been evaluated
% and are stored in the array 'pc'.

if (np < ncf)
    for i = np + 1:1:ncf
        pc(i) = twot * pc(i - 1) - pc(i - 2);
    end

    np = ncf;
end

bcoef = ncf * na * ncm;

cbody = zeros(bcoef, 1);

n = ibuf;

for m = 1:1:bcoef
    cbody(m) = coef(n);
    n = n + 1;
end

cbuf = zeros(ncf, ncm, na);

n = 0;

for l = 1:1:na
    for i = 1:1:ncm
        for j = 1:1:ncf
            n = n + 1;
            cbuf(j, i, l) = cbody(n);
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% interpolate to get position for each component
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:ncm
    pv(i, 1) = 0;

    for j = ncf:-1:1
        pv(i, 1) = pv(i, 1) + pc(j) * cbuf(j, i, ll);
    end
end

if (ifl <= 1)
    return
end

% if velocity interpolation is wanted, be sure enough
% derivative polynomials have been generated and stored.

vfac = (dna + dna) / t(2);

vc(3) = twot + twot;

if (nv < ncf)
    for i = nv + 1:1:ncf
        vc(i) = twot * vc(i - 1) + pc(i - 1) + pc(i - 1) - vc(i - 2);
    end

    nv = ncf;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% interpolate to get velocity for each component
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:ncm
    pv(i, 2) = 0;

    for j = ncf:-1:2
        pv(i, 2) = pv(i, 2) + vc(j) * cbuf(j, i, ll);
    end

    pv(i, 2) = pv(i, 2) * vfac;
end

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

function fr = split(tt)

% this function breaks a number into a integer
% and a fractional part.

% input

%   tt = input number

% output

%   fr = 2-word output array
%        fr(1) contains integer part
%        fr(2) contains fractional part

%        for negative input numbers, fr(1) contains the next
%        more negative integer; fr(2) contains a positive fraction.

fr = zeros(2, 1);

fr(1) = fix(tt);

fr(2) = tt - fr(1);

if (tt >= 0 || fr(2) == 0)
    return
end

% make adjustments for negative input number

fr(1) = fr(1) - 1;

fr(2) = fr(2) + 1;


function pout = propagate_bounds_from_arbitrary_quadratics(p)

pout = p;
if p.bilinears~=0
    F_struc = p.F_struc;

    p.F_struc = [-p.F_struc(1:p.K.f,:);p.F_struc];
    p.K.f=2*p.K.f;

    InequalityConstraintState = [p.EqualityConstraintState;p.EqualityConstraintState;p.InequalityConstraintState];

    if p.K.l+p.K.f>0
        quadratic_variables = find(p.bilinears(:,2) == p.bilinears(:,3));
        if ~isempty(quadratic_variables)
            quadratic_variables = p.bilinears(quadratic_variables,1);
            for i = 1:length(quadratic_variables)
                k = quadratic_variables(i);

                if (p.lb(k) < p.ub(k)-1e-4)
                    x = p.bilinears(p.bilinears(:,1)==k,2);% x^2
                    candidates = find((InequalityConstraintState==1) & p.F_struc(1:p.K.f+p.K.l,1+k))';
                    for j = candidates
                        a = p.F_struc(j,2:end);
                        b = p.F_struc(j,1);
                        aij = a(k);

                        % rest + cij*x + (+-)x^2 >= 0
                        cij = a(x);
                        cij = cij/abs(aij);
                        b = b/abs(aij);
                        a = a/abs(aij);
                        %  aij = sign(aij);

                        a(k) = 0;
                        a(x) = 0;
                        indNEG = find(a < 0);
                        indPOS = find(a > 0);
                        LB = p.lb;
                        UB = p.ub;
                        LB(k) = 0;
                        UB(k) = 0;
                        LB(x) = 0;
                        UB(x) = 0;
                        a(k) = 0;
                        a(x) = 0;

                        if aij < 0
                            % rest + cij*x - x^2 >= 0

                            % Derive upper bound on rest
                            rest = (b+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)]);
                            % Write as rest >= (x - center)^2-(center)^2
                            center = cij/2;
                            radii2 = rest + (cij/2)^2;
                            if radii2 > 0
                                newUB = center + sqrt(radii2);
                                newLB = center - sqrt(radii2);
                                p.lb(x) = max(p.lb(x),newLB);
                                p.ub(x) = min(p.ub(x),newUB);
                            end
                        else
                            % rest + cij*x + x^2 >= 0
                            % (x+cij/2)^2 - (cij/2)^2 + rest >= 0
                            % (x+cij/2)^2 >= radii
                            % Lower bound on rest
                            rest = (b+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)]);
                            center = -cij/2;
                            radii2 = -rest + (cij/2)^2;
                            if radii2 > 0
                                left  = center-sqrt(radii2);
                                right = center+sqrt(radii2);
                                if p.ub(x) < right
                                    p.ub(x) = min(p.ub(x),left);
                                end
                                if p.lb(x) > left
                                    p.lb(x) = max(p.lb(x),right);
                                end
                            end
                        end
                    end

                elseif p.lb(k)<0

                end
            end
        end
    end

    if p.K.l+p.K.f>0
        bilinear_variables = find(p.bilinears(:,2) ~= p.bilinears(:,3));
        if ~isempty(bilinear_variables)
            bilinear_variables = p.bilinears(bilinear_variables,1);
            for i = 1:length(bilinear_variables)
                k = bilinear_variables(i);
                if p.lb(k) >= -5000000000 & (p.lb(k) < p.ub(k)-1e-4)
                    x = p.bilinears(p.bilinears(:,1)==k,2);% x^2
                    y = p.bilinears(p.bilinears(:,1)==k,3);% x^2
                    candidates = find((InequalityConstraintState==1) & p.F_struc(1:p.K.f+p.K.l,1+k))';
                    for j = candidates
                        a = p.F_struc(j,2:end);
                        aij = a(k);
                        if aij > 0
                            indNEG = find(a < 0);
                            indPOS = find(a > 0);
                            LB = p.lb;
                            UB = p.ub;
                            LB(k) = 0;
                            UB(k) = 0;
                            a(k) = 0;
                            newLB = (-p.F_struc(j,1)-a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)])/aij;
                            p.lb(k) = max(p.lb(k),newLB);

                        elseif aij < 0
                            indNEG = find(a < 0);
                            indPOS = find(a > 0);
                            LB = p.lb;
                            UB = p.ub;
                            LB(k) = 0;
                            UB(k) = 0;
                            a(k) = 0;
                            newUB = (p.F_struc(j,1)+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)])/(-aij);
                            p.ub(k) = min(p.ub(k),newUB);
                        end
                    end

                    if p.lb(k)>0 & p.lb(x)>0 & p.lb(y)>0
                        p.ub(x) = min(p.ub(x), p.ub(k)/p.lb(y));
                        p.ub(y) = min(p.ub(y), p.ub(k)/p.lb(x));
                        p.lb(x) = max(p.lb(x), p.lb(k)/p.ub(y));
                        p.lb(y) = max(p.lb(y), p.lb(k)/p.ub(x));
                    end

                elseif p.lb(k)<0

                end
            end
        end
    end
end

if ~isequal([p.lb p.ub],[pout.lb pout.ub])
    quad_v = find(p.bilinears(:,2) == p.bilinears(:,3));
    quad_x = p.bilinears(quad_v,2);
    quad_v = p.bilinears(quad_v,1);
    if ~isempty(quad_v)
        % y = x^2, x>=0, y >= L means x >= sqrt(L)
        k = find(p.lb(quad_x)>=0 & p.lb(quad_v)>0);
        if ~isempty(k)
            p.lb(quad_x(k)) = max(p.lb(quad_x(k)),sqrt(p.lb(quad_v(k))));
        end
    end
end

if ~isequal([p.lb p.ub],[pout.lb pout.ub])
    pout.changedbounds = 1;
end
pout.lb = p.lb;
pout.ub = p.ub;